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I. INTRODUCTION 


It is estimated that nearly half of the Earth's land surface is inaccessible to 
wheeled and tracked vehicles [Ref. 1]. Yet almost all of this same area can be 
successfully traversed by animals and man. This great difference in mobility has 
motivated research into the creation of a practical legged vehicle or 
walking machine. 

The advantages of legged locomotion can largely be attributed to the 
flexibility offered in leg placement and support. Wheeled vehicles. and to a lesser 
extent tracked vehicles. are confined to a more or less continuous. relatively flat 
and obstruction free paths along the ground. The leg’s flexibility allows the 
utilization of discontinuous support regions on the ground and the adaptation to 
terrain slope. A legged vehicle may potentially use obstructions for support as it 
climbs over those obstacles which it decides to not simply ignore. 

A second advantage of legs involves the means of obtaining traction in soft 
soil. A wheel or track creates a depression or rut from which it must continually 
work to climb out. Slippage causes the wheel or track spin. possibly digging a 
deeper hole. A leg, however, may be lifted vertically out of its depression, 
minimizing the work required. In addition. any back slip caused by the vehicle 


stepping pushes up soil behind the foot and improves traction. [Ref. 2] 


=1 


The combination of flexible coordination and increased traction provides a 
potential for greater speed and less power consumption while operating over rough 
and otherwise unsuitable terrain. Other advantages of legged locomotion include 
possible improved comfort in ride due to the adaptive nature of legged support on 
uneven terrain. the ability to test soil conditions prior to placement of weight on 
the legs. and the relatively small footprint left in the soil. The latter may prove 
especially important for agricultural work, where the disturbance of crops is to be 
minimized, or for military vehicles navigating areas suspected of containing 


landmines. 


Ae GOL 

The purpose of this study is to explore a new type of gait and steering 
algorithm for the use of legged walking machines. The gait is a particular type of 
tripod gait. which can be considered as an extension of the temporal follow-the- 
leader gait ‘Ref. 3]. into the spatial domain. The steering algorithm to be 
investigated along with this style of gait borrows from the concept of driving a 
wheeled tractor-trailer vehicle. It is believed that this steering algorithm may be 
particularly well suited for the fixed foothold position requirements of follow-the- 
leader gaits. 


The machine chosen as a physical reference for the study is the Adaptive 


Suspension Vehicle (ASV). This is a self-contained. six-legged vehicle currently 


being evaluated at the Ohio State University for rough-terrain locomotion. The 
ASV is a Defense Advanced Research Projects Agency (DARPA) proof of concept 
project. 

A secondary goal is to develop a simulation model with which to study 
walking gaits and control algorithms in general for the ASV. This model is 
developed along the general lines of the simulation previously presented by Lee 
Ref. 4], incorporating several of his model's features, including omni-directional 
control. foot movement. and body attitude and altitude regulation algorithms. In 
addition. this simulation is to have the features of operation in either the new 
follow-the-leader tripod gait mode or in Lee’s "forward wave" tripod gait mode, 
an enhancement of realism with a detailed color graphics display, and a menu 


system controlled with a single mouse button. 


mB ORGANIZATION 

Chapter II provides a brief overview of the previous work relating to this 
study. It includes a discussion of state of the art legged vehicles, tripod follow- 
the-leader gaits, tripod gaits, stability and simulation displays. 

A detailed discussion of the ASV simulation problem is presented in Chapter 
Ill. This chapter covers the configuration of the vehicle, the gait and steering 
algorithms. the simplifications assumed in the construction of the model, and the 


kinematics involved in making the ASV model walk. The final section in this 


chapter describes the IRIS-2400 simulation hardware and software on which the 
model was developed. 

The simulation program's operation and functions are presented in Chapter 
IV. This includes a complete description of the operation of the program controls 
and display features. This is followed by a discussion of the means by which 
graphics are programimed on the IRIS. and by a description of the organization 
and flow of the program and its modules. 

Chapter V is a review of the performance of the simulation. It includes a 
brief subjective view on the feel of driving in the two modes. 

The final chapter summarizes the contributions of this study. It also contains 
cominents on possible directions for future research. The program code is listed in 


the appendix. 
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Il. SURVEY OF PREVIOUS WORK 





Pee lN ERODUCTION 

The last quarter of a century has witnessed intense efforts to build machines 
that walk. Difficulties facing researchers include the problems of controlling the 
mimany degrees of freedom necessary in a maneuverable leg. maintaining vehicle 
stability, creating energy efficient motion. and adapting the walking motions to 
unstructured terrain. With the advent of compact computer technology and 
computer-aided simulation and design, serious progress is now being made in 
overcoming these problems. [Ref. 5] 

Several promising working designs have emerged in the last ten years. Some 
of the most prominent include the Perambulating Vehicle I] (PVII) at the Tokyo 
Institute of Technology. the Carnegie-Mellon University hexapod. the Odetics Inc. 
ODEX I. and the Adaptive Suspension Vehicle (ASV) developed at the Ohio 
State University. 

The PVII is a light-weight laboratory model quadruped. developed in 1980. 
It features one of the first pantograph leg constructions designed specifically to 
provide simplified leg coordination and energy efficient walking. Using tactile foot 
sensors and a microcomputer mounted near the vehicle. the PVII is able to probe 


for footholds and maneuver over obstacles. [Ref. 6] 
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The hexapod developed at the Carnegie-Mellon University in 1982 is a self- 
contained walking machine large enough to carry its operator. It uses a gasoline 
engine to provide power to the legs via a set of hydraulic actuators. The 
movements of the individual legs are controlled by a series of passive hydraulic 
circuits. A built-in microprocessor interprets the driver's commands and specifies 
the correct series of leg movement patterns to be used. This arrangement frees 
the single microprocessor from the need to compute each foot trajectory. [Ref. 7] 

The ODEX I is a commercial design introduced in 1983 [Ref. 8]. An 
improved version. sometimes referred to as ODEN II, is being developed for near- 
term use in nuclear power plants [Ref. 9]. The ODEX series makes use of a 
unique circular arrangement of six planar pantograph legs which allow the 
vehicles to adjust their profile for negotiation of narrow passages. The ODEX 
walking machines are directed through a radio or fiber-optic link from the 
operator to an on-board supervisory-level microprocessor. Each leg is controlled 
by a dedicated lower-level microprocessor which receives instructions from the 
supervisory level microprocessor. The new ODEX hexapod is also being equipped 
with a center-mounted arm for remote manipulation of objects, such as valves, in 
hazardous environments. [Ref. 8] 

The Adaptive Suspension Vehicle (Figure 2.1), currently being tested at Ohio 
State University. is the first computer-coordinated legged vehicle designed and 
built for operation on natural terrain [Ref. 10]. This hexapod walking machine is 


completely self-contained. and is capable of carrying the driver, a 500 |b. internal 
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pavload. computer and contro] circuitry. and power system. in an outdoor 
environment. The ASV is the vehicle modeled in this study. A more detailed 
description of the ASV follows in Section 3.B. 

The remaining sections of this chapter concern gaits used by walking 
machines. vehicle steering. the walking machine stability problem. and graphical 
representation of the vehicle's motion. The gait and stability sections are oriented 


towards six-legged vehicles such as the ASV. 


B, GATT sELE Se ROM 


ie Definitions 





A gait is a mode of locomotion for a vehicle or animal distinguished by a 
specific pattern of lifting and placing of the feet. Gaits in general may be 
described using the event sequence notation introduced by McGhee and Jain |Ref. 
11). The integer 7 in such a sequence corresponds to the event of placing foot 7 on 
the ground. The lifting of the same foot is represented by the integer 7? + n. 
where nm equals the number of legs. For the ASV, legs are numbered on the left 
side (1. 3. 5) from the front to the rear, and on the right side (2. 4. 6) in the same 
order. 

A ‘periodic gait is one that repeats the lifting and placing pattern. and 
thus is represented by one cycle of events. A periodic gait is said to be 
nonsingular if no two of its events occur simultaneously. McGhee [Ref. 12] 


demonstrated the existence of 39.916.800 possible nonsingular periodic hexapod 
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caits. The total number of possible hexapod gaits is a much larger and unknown 
number |Ref. 5]. This makes the selection of an optimum gait a very difficult 
problem. However, this thesis is concerned with a single type of gait sequence, 
the tripod sequence. These are singular gaits, in that more than one leg is placed 
at a given instant (Ref. 3]. 

A periodic gait is considered symmetrical when the stepping pattern on 
one side of the body is identical to that on the opposite side and separated in time 
by exactly one-half of the gait period |Ref. 12]. Symmetry tends to simplify the 
required leg coordination algorithms. 

The pitch of a gait is the distance between footholds, measured in body 
lengths (defined as the distance between the front and rear leg reference 
positions). Leg stroke is the linear distance the foot travels with respect to the 
body when occupying a particular foothold. Leg stroke is also expressed in termis 
of body lengths. 

2. Follow-the-Leader Gaits 

A follow —the—leader(FTL) gait is one in which the middle and rear legs 
on each side of the body step in the foothold locations previously occupied by the 
leading legs (Ref. 13]. Creeping FTL gaits (in which at most one leg is in the air 
at any time [Ref. 14]). were first studied by Ozguner, Tsai, and McGhee [Ref. 3]. 
Using a temporal framework. they narrowed the number of possible FTL creeping 


gaits to 30, of which they found five to be symmetrically realizable. 
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Expanding to a spatial reference frame greatly increases the number of 
gaits in this category. A tripod creeping gait can be defined as one in which the 
legs are placed in alternating groups of three. with each group forming a tripod of 
support. In the case of the ASV. the two possible tripods are the leg sets (1 4 5) 
ame al 2 Ole 

The possible distinct tripod creeping gaits can be ennumerated using an 
approach similar to that in Ozguner et al. [Ref. 3]. Choosing the placement of leg 
1 as a reference. evidently there are two possibilities for the relative ordering of 
legs 4 and 5, and three possible locations in each sequence for the insertion of the 
alternate group of legs. Furthermore, the placing of the alternate leg group (2 3 
6) can be accomplished in six distinct ways. Table 2.1 lists the 36 possible 
nonsingular placing sequences. 

It might first appear strange that the sequence ( 1 2364 5) is included 
in Table 2.1. However taking two periods together. the sequence becomes 
(1236451236435), which clearly shows that the placement of the legs 
occur in alternating groups of three. 

Comparing the entries in Table 2.1 to those in the table of Ozguner et al., 
one can see that none of these sequences are listed in the latter work. This is 
because the sequences here are not temporally follow-the-leader. Yet they all are 
spatial FTL gaits. This can be seen from the gait kinematics of the example 


shown in Figure 2.2. 
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TABLE 2.1. PLACING SEQUENCES FOR TRIPOD CREEPING FTL GAITs | 


Gait | Placing , Tripod 2 — ‘Tripod i . ~ Tripod Z 
Number Sequence Insertion Subsequence Subsequence 





















































Position 
1 123645 1 145 
2 126345 1 145 
3 132645 1 145 
i 136245 1 145 
5 162345 1 145 
6 163245 1 145 
ji 123654 1 154 
8 126354 1 154 
9 132654 l 154 
10 36254 1 154 
i 162354 1 154 
12 163254 1 154 
13 142365 2 145 
14 142635 2 145 
143265 ? 145 
143625 2 145 
146235 2 145 
146325 2 145 
152364 2 154 
; 152634 2 154 
21 153264 2 154 
29 153624 2 154 
23 156234 9 154 
De 156324 2 154 
25 145236 3 145 
26 145263 3 145 
Dy 145326 3 145 
28 145362 3 145 
145623 3 145 
145632 3 145 
154236 3 154 
me 154263 3 154 
154326 3 154 
154362 3 154 
154623 3 154 
154632 3 154 
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It is possible to alternate body motion with leg placement in the tripod 
gait (Figure 2.3). This yields a pattern of movement that 1s compatible with the 
general notion of a creeping gait |Ref. 3]. It should be noted, however. that while 
such a strategy may improve the static stability of the gait [Ref. 3], the 
intermittent body motion increases the leg stroke by a factor of two, which 
greatly increases the required working volume of the legs. For this reason. and 
also because intermittent body motion slows the average vehicle forward speed. 
only the continuous body motion alternative will be considered further in this 
thesis. 


2) 


3. Singular Tripod Gaits 





Tripod gaits have proved to provide a good compromise between 
stability. maneuverability. and ease of control for the Ohio State University 
Hexapod. the ODEX I, and the ASV. For this reason tripod gaits were chosen for 
this simulation study. 

It can be seen that a tripod gait is actually a special limiting case of a 
creeping gait. where the time between the placement of individual legs within a 
tripod grouping approaches zero. Of the very large (unknown) number of gait 
sequences possible. only one can be classified as a singular tripod gait sequence. 
All differences among varieties of tripod gaits are therefore a function of 


i 


kinematics only. 
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The most frequently used tripod gait is the limiting form of the 
forward wave gait, where the duty cycle', 3. approaches Weiner 4.5.) [his 
study introduces the singular FTL tripod gait. Both of these gaits are 
implemented in the walking algorithms of this simulation and are described 
further in Chapter III. 

It is interesting to note that potentially the fastest forward wave tripod 
gait for the ASV is an FTL tripod gait with a pitch of one (Fig. 2.4). This of 
course can only be considered a true FTL gait if the feet are assumed to be 
dimensionless. In order to prevent the legs from interfering with one another, the 
duty cycle might be made slightly less than 1/2. This would momentarily leave 
the vehicle with no supporting legs in contact with the ground. It would also 
have the disadvantage of not providing sufficient time for possible foothold 


searches by the leading legs. 


we SLEERING 

There are several different approaches to steering currently used by ground 
vehicles. The most familiar method is articulated, or automotive style steering 
[Ref. 15]. With a steering wheel, accelerator and brake, the driver of an 


automobile can directly control the vehicles turning radius and forward velocity. 


i 


' The duty cycle is the fraction of the leg cycle used for supporting the body. 
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Tracked vehicles. on the other hand. most frequently utilize skid steering. By 
operating the sets of tracks at differing rates. the driver controls the turning rate 
and forward velocity of the vehicle. 

Tractor-trailers use still another type of steering. Here the driver steers far 
forward of the vehicle's center of gravity. The trailer follows along in the path of 
the cab. with the steering of its center of gravity lagging behind the steering of 
the cab. Furthermore. since the trailers wheel axle orientation constrains its 
motion, the trailer is restricted to a larger turning radius than the cab is capable 
of steering. 

Specially designed wheeled vehicles may use omni-directional steering |Ref. 
16]. This rarely used method allows the driver to specify turning rate and 
velocity in any horizontal direction. 

Legged vehicles have historically used similar steering approaches. McGhee 
and Iswandhi [Ref. 17], introduced a two-axis joystick control, analogous to 
articulated steering, in which one axis controlled the turning radius and the other 
controlled forward velocity. Orin [Ref. 18]. applied three-axis joystick control to 
the Ohio State University Hexapod, a small laboratory scale walking vehicle. 
This allowed forward, lateral and rotational velocities to be specified bv the 
driver. providing steering control much like that experienced in a helicopter. The 
current ASV uses a similar three-axis joystick control. 

Tractor-trailer style steering has not yet been applied to walking vehicles. 


This approach, which will be developed in this thesis. should give improved two- 


Paes 


axis control to the driver for moving through areas of restricted maneuverability. 
The driver need only be concerned with maneuvering the front end of the vehicle. 
The body of the vehicle will follow along the proven path established by the 


footholds used by the front pair of legs. 


D. SstABI elias 

The problem of vehicle balance is a vital concern for walking machines and 
has been a focus of many studies. Legged vehicles may maintain their stability 
using one of two methods. static balancing [Ref. 19] or dynamic balancing [Ref. 
20]. 

Static stability is attained by maintaining the vertical projection of the 
vehicle's center of gravity within the polygon defined by the supporting legs |Ref. 
5]. This method is conceptually simple. It is. however, only valid for stationary 
or slow moving vehicles. as it neglects the effects of inertia on stability. 

Dynamic balancing is a complex process which places fewer restrictions on 
vehicle velocity. The vehicle may be allowed to momentarily move into a 
statically unstable configuration. so long as. over time. an adequate base of 
support is provided [Ref. 20]. This is the mode of balancing normally used by 
man and most vertebrate animals. It remains an extremely complex process. 
however. which is difficult to reproduce with legged vehicles. 


This model uses only the static criteria for stabilitv. Having the vehicle 


limited to reasonable velocities and the six legs placed in alternating tripod 
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support patterns ensures a high degree of stability. To guarantee stability. the 
usable working volume for each leg is reduced. Figure 2.5 shows a worst case 
situation demonstrating that. if the legs are confined to their respective 
constrained working volumes, the vertical projection of the center of gravity will 
always fall within the triangular pattern formed by the supporting legs. A further 


discussion of the constrained working volume can be found in |Ref. 4]. 


Ee GRAPHICS 

There is a wide spectrum of available options from: which to choose in the 
field of graphic displays. Decisions are required as to running the simulation on 
monochrome or color monitors, the type and number of dimensions for the 
projection, the use of line or solid figure representation. acceptable display 
resolution and update time. and whether to employ special hardware options. 
State of the art graphics machines also offer possibilities which include shading, 
reflectivity of surfaces, and multiple light sources. A compromise must be made 
between functionality. visual realism, and cost in order to realize an effective 
simulation. 

Past simulation models featuring the ASV |Ref. 4.21.22] have concentrated on 
basic functionality in the display. The vehicles and terrain were represented by 
simplified line drawings on a monochrome monitor. This study attempts to take 


advantage of recent developments in special hardware and software for graphics 


workstations. in order to create a more realistic and convincing simulation. It is 
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Pireure 2.9 Constrained Working Volumes 
for Adaptive Suspension Vehicle 
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simulation. It is believed that the IRIS-2400 [Ref. 23] represents a good 
compromise between state of the art quality. cost. processing time and 


availability. This svstem was therefore selected to support the work of this thesis. 


be SUMIMARY 

This chapter provides background information on previous research leading to 
this study. Discussions include a brief survey of examples of the state-of-the-art 
walking machines. follow-the-leader and tripod gaits. vehicle steering, and the 
question of stability for walking machines. In addition. several concerns are 
expressed regarding the graphics displays used to portrav the action of the 
walking vehicles. 

The following chapter contains a detailed statement of the ASV simulation 


problem to be solved in this thesis. 
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III. DETAILED PROBLEM STATEMENT 


A. INTRODUCTION 

This chapter is intended as a description of the nature of the simulation 
modeling problem. In it is a discussion of the configuration of the ASV, the 
mathematics governing its motion, and foothold selection and steering algorithms 
for two selected gaits. Also covered are the simplifications deemed necessary in 
the creation of the model. The final section includes a brief description of the 


modeling facilities. 


B. AS\Y CONFIGURATION 

The Adaptive Suspension Vehicle (ASV) is a_ self-contained. six-legged 
walking machine designed to traverse uneven terrain. The operator. sitting in a 
cockpit at the front of the vehicle, controls the vehicle either at a supervisory level 
by selecting body translational and rotational velocities and allowing the vehicle 
to automatically place the feet. or by coordinating the individual legs in a 
precision-footing mode. The various control modes are discussed in |Ref. 10]. 

The vehicle is equipped with an optical scanning rangefinder. mounted above 
the cab. oe aiaeeanee sensing. The laser rangefinder has a range of 10 m and a 


field of view of 40 degrees on each side of the body axis, and from 15 to 75 degrees 


below the horizontal. |Ref. 10: p.8] 


A single 900 cc four-cylinder motorcycle engine is sufficient to power the ASV 
over sustained periods of time. This is possible due to the aluminum construction 
of the frame and legs. which make the vehicle relatively light (2700 kg) for its size 
(3.0 m height, 5.2 m length) [Ref. 10:pp. 8-10]. Power is distributed to eighteen 
hydraulic actuator pumps through an energy storage flywheel and a series of 
shafts and toothed belts. 

Seventeen Intel 86/30 single-board computers are used for onboard processing 
and control. One board is dedicated to each leg for motion control and leg sensor 
data processing. Four more boards compute stability, check actuator motion 
limits. and generate leg commands based on the operators control inputs and the 
internal terrain model. Two additional boards are used for cockpit displays and 
controls. The terrain model is generated by the remaining five single-board 
computers using the data gathered from the optical rangefinder. |Ref. 10: pp. 8- 
10] 

The design of the ASV’s legs features a two-dimensioned pantograph 
mounted on a baseplate hinged to the body (Fig. 3.1 and 3.2). This design offers 
the advantages of energv efhiciency resulting from decoupled ground reaction force 
components, and simplicity of control [Ref. 5.6, and 24]. Vertical and horizontal 
motion relative to the baseplate are provided by independent actuators mounted 
to the plate. Abduction and adduction motion is provided by a third actuator 


mounted on the body. 
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C. SELECTED WALANG ALeGC aa 
1. Gaits 

The model used in this simulation study currently supports two styles of 
walking patterns. The first, closely following that used by Lee [Ref. 4] features a 
periodic tripod forward wave gait. The second is a_ periodic tripod 
follow—the leader (FTL) gait [Ref. 3]. Both gaits have unique advantages to 
offer the operator. 

The great advantage inherent in the forward wave gait les in the 
maneuverability it offers the walking vehicle. The ASV. operating in the forward 
wave gait mode. is free to place its feet anywhere within a constrained working 
volume during the leg placement phase of the walking cycle [Ref. 4:pp.59-62]. 
This freedom allows the vehicle great flexibility in range of movement: even to the 
point of permitting turning in place. 

The price for this freedom of choice for leg placement is that a foothold 
must be found and tested for each time a leg is placed on the ground. In rough or 
obscured terrain the process of probing and testing could occupy virtually all of 
the vehicle’s onboard processing capability. Thus, the vehicle's speed over ground 
could be severely limited. 

In this type of terrain the follow-the-leader gait could prove more 
advantageous. The follow-the-leader gait requires probing and testing only for 


the forward two legs. Since the following legs step precisely where the leading 


legs have gone. no further searching is needed. On difficult or dangerous terrain. 
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where extensive probing and testing of foothold is required, the FTL gait promises 
both greater ground speeds and more security. 

The notable disadvantage of the FTL gait is that its use drastically 
constrains the vehicle's movement. Maneuvers such as sideways stepping and 
turning in place are not possible. Turning the vehicle requires a large radius 


turning circle, similar to that needed by a tractor pulling a long trailer. 


to 


Steering 

The two walking algorithnis utilize different control schemes matching the 
unique gait characteristics. The forward wave gait steering mode allows the 
operator to independently specify longitudinal velocity, lateral velocity. and 
azimuth angle rate (ideally using a three-axis joystick). This allows the operator 
to take fully advantage of the gait’s maneuverability. In the absence of a three- 
axis joystick for this simulation. these body translation and rotation rates are 
input through three sliding bar controls using a mouse-driven cursor on the 
display screen. 

The vehicle in the follow-the-leader gait mode. with its inherent 
restriction that the body remain between the two parallel foothold tracks, behaves 
very much like a tractor and trailer or a wagon. Just as the truck driver steers 
the cab allowing the trailer to follow in its path, the ASV operator in this mode 
steers by specifying the desired motion of the vehicle steering point. This point 


lies just behind the cockpit, mid-way between the two front legs, along the line 


joining the centers of the two working volumes. In the place of a steering wheel 


Do 


and acceleration pedal. the operator uses a two-axis joystick (simulated with a 
mouse-driven cursor on a steering pad), to specify the desired magnitude and 
direction of the cockpit’s relative velocity vector. 

The truck and trailer or wagon style steering commands are translated 
into desired longitudinal and lateral translation and azimuth rotation rates in 
order to maintain compatibility with the wave gait control algorithm in the 
program. This is done by first transforming the steering point (vehicle head) 
actual position and desired velocity to Earth coordinates. (r,p. Y¥,p.. 2% ,) and 
(Tap. Yanr- Zapp) Tespectively. The desired cockpit position (=), —, 9); .s=a ummm 


determined by 


Ee - “pe = lobe 7 Al (3.1) 
Yone = Yar * Yank >! (3.2) 
cake ~ “he * “ane” —* (3.3) 


where At is the program display time increment. Using the desired cockpit 
position and the centroid of the middle and rear legs’ footholds {in Earth 


coordinates) (aD opt the desired azimuth angle WU, is obtained. 


- YahE fh, 
VS = tan ao ae (3.4) 
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The desired new position of the body's center (2, 5-Yjp- Zge¢)+ 18 then found by 


ie 
lap = Iy,p — cos¥, [3-9} 
) 
L . 
Yae ~ Yahe Tet (3.6) 
a - 2p et) 


where L is the length between the center of the working volumes of the forward 
and rear legs. 
The desired Earth translation rates (z,, and y,,) and Euler azimuth 


angle rate (w,,,) are determined as 


“dE “E (3.8) 
a Loa 
it 
. Yae OYE , 
YaE (3.9) 
ee: 
vv 
a (3.10) 
Oey 


with W being the current azimuth angle. These Earth and Euler rates are then 


itanslated to body rates (z,,, ¥,p. 4,9) bY 


i = 2,,c08v + y,psinY (Beh) 
i v7,cosY — Z, sin’ (Bou) 
Wien ~ “dep (3.13) 
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o. Foothold Selection 





As indicated in the above section. a new foothold must be selected for 
each leg while operating in the forward wave gait mode. In order to maximize the 
foothold’s usefulness it should be placed so that the foot remains in the 
constrained working volume during the leg s support phase for a maximum length 
of time. The optimal foothold position 1s determined as the "point on the surface 
of the constrained working volume such that |the leg’s] support trajectory is 
predicted to pass through the foot reference position" [Ref. 4: p.100]._ To simplify 
the computation, the reference position is taken as the center of the working 
volume and a straight line is used to approximate the foot trajectory. A line is 
projected opposite to the direction of the predicted foot velocity vector at the 
reference point. The intersection of this line and the boundaries of the 
constrained working volume is then the desired foot position. Subsequent 
variation of the body velocity will alter the supporting foot trajectory, potentially 
resulting in a suboptimal foothold. 

The follow-the-leader gait foothold selection process is much different. 
New footholds for the leading two legs are found by projecting a line along the 
ios vector of the vehicle’s cockpit. At a set distance (1/12 the length 
between the forward and rear hip joints), along this line, another line 


perpendicular to it is projected. This distance is one half the leg stroke of the 


vehicle while operating with a pitch of 1/3 (Figure 2.2). 
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The desired foothold is determined by where the second line intersects a line 
running through the center of the working volume parallel to the body’s 
longitudinal axis (Figure 3.3). 

As a front leg abandons its current foothold. that position is recorded for 
use by the middle leg behind it. In turn. the middle leg foothold positions are 
saved for use by the rear legs. Thus, during each complete leg cycle, two new 
foothold positions are computed. This compares favorably to the six new 


footholds needed while using the forward wave gait. 
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foothold longitudinal 
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Figure 3.3 New Foothold Location 


The current program allows the driver to start operation of the vehicle 
either using the forward wave gait or the follow-the-leader gait. Once started. the 


program wulust be reset before switching modes. 


Doe XIODELING SINR DIFICAT IGA 

There are many simplifving assumptions contained within this model of the 
ASV. These simplifications were made largely in an effort to speed the 
development of such features as the follow-the-leader gait. However, the program 
framework was devised with future work in mind. Thus. wherever possible room 
was left for generalization and expansion. 

The most notable simplification in the simulation model deals with terrain. 
The ground is represented by a smooth. level. checkerboard pattern. Although 
the ASV was developed to be able to traverse unstructured terrain. there are no 
obstacles or obstructions in the current model. Uneven terrain will require 
inclusion of an algorithm for estimating the support plane beneath the vehicle, 
foot sensors, and a new terrain display routine. A foothold probe and testing 
routine will also be needed. 

As a consequence of the use of flat terrain. the constrained working volume 
adjustments for uneven terrain |Ref. 4: pp. 109-117] and body regulation plans for 
varying slopes [Ref. 4: pp. 87-89] were not required. However, the basic structure 


for body attitude and altitude regulation has been retained in the program 
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modules of this thesis. Consequently. inclusion of sloped terrain should 
necessitate only minor program changes in these areas. 

The model contains only kinemnatic features of the ASV operation. This 
means that there are no limits on vehicle acceleration imposed by the model. In 
order to prevent unrealistic performance on the part of the displayed vehicle, a 
filter was placed between the commanded inputs and the response of the vehicle. 
The kinematics and filter for simulating dynamic constraints are described in the 


section below. 


PeeODEL KINEMATICS 

The kinematics of the model of the ASV presented here closely follow those 
developed in the computer simulation of Lee [Ref. 4]. Body motion is specified in 
terms of translation rates along the body's forward, lateral, and vertical axes (x. 
y. and z repectively) and rotation rates around these axes. The driver of the 
vehicle may directly or indirectly control the desired forward and _ lateral 
translation rates and the rotation rate around the vertical axis. The remaining 
three degrees of freedom are automatically regulated to maintain a desired body 
attitude and altitude with respect to the ground. 

Vehiclé dynamics are simulated through the use of a simple control filter 


inserted between the ordered rates and the actual body rates. Asa result the 
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body moves with a smooth. exponential transition in response to driver and body 
regulator control commands. 

In order to realize these filtered body rates. the rates are first converted to 
earth coordinate translation rates and body Euler angle rates. Euler integration is 
then performed to produce translation distances in earth coordinates and angular 
displacement around the three body Euler axes. 

1. Coordinate Systems 


The ASV model makes use of two coordinate systetns, earth (Zp. yp, Zp) 
and body (4. yg. 2g). in its calculations. The earth coordinate system is used 
wherever it 1s required to specify absolute position or velocities of the body, feet. 
or terrain. The earth coordinate system is defined such that the z, axis is positive 
upward and the unit vectors r,, yp, and z, are mutually orthogonal. 

The body coordinate system is useful in describing operator control and 
the coordination of body and legs. The origin is defined as the center of the main 
body section (excluding the cockpit). The z, axis is projected upward through 
the top of the body, while the r, axis is forward along the longitudinal axis and 
the y, axis is projected to the body's left. forming the transverse or lateral axis. 


Earth coordinates are transformed to body coordinates using the 


relationship of equation 3.14. 


40 


y y 

Oe care (eas (3.14) 
~ “B 

l l 


where the position vectors |[r,. Yp. Zp: i)" eve Clig eet eer ihe describe the same 
point in space in earth and body coordinates respectively and H is a 4 x 4 
homogeneous transformation matrix [Ref. 25]. The homogeneous transformation 
matrix can be derived by decomposing the transformation into a translation from 


the earth coordinate origin and a series of rotations about the Euler axes: 


ao-— J oT 


TyzZ v r 


oF (3ak5) 


The homogeneous transformation matrix io represents the translation 
of the body’s center to its current position (d,. d,. d_). The first rotation about 
the body's vertical axis by the azimuth angle W is represented by the matrix Ty. 


The body is then rotated about its new lateral axis by the elevation angle. ®. and 


then about the newly formed longitudinal axis by the roll angle. 0.! 


' Other notations are sometimes used for these angles. For example, in |Ref. 19], @ signifies 
elevation angle and @ denotes roll angle. 
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The four homogeneous transformation matrices [Ref. 25: p30] are: 


T (3.16) 
aus O70 ea 


00 0 1 


cosW — sin O 


sinW cosV¥ O O 


r (3.17) 
¥ 0 0 14 
0 QO O 1 
cos® O sin® O 
O 1 0 O 

(h (3.18) 
® -~sin® 0 cos® O 


1 O 0 0 
Q cosO sinO9 0 

ae (3.19) 
0 0 Sem cosO G 


0 0 Ot 


EWostiutmecauations 6.10, 0,17, 3.16. and 3.19 into 3.15 yields: 


cosUcos® cosVsin®sinO—sinYcosO sinVsinO +cosVsin®cosO d_ 


sinVcos® cosVcosO+sinVsin®sinO sinVsin®cosO—cosVsinO ny, 


16 i (620) 
=o cos®sinO cos®cosO d 


ra 


0 0 0 1 


2. Body Regulation 
A simple control algorithm is used in this and Lees model to maintain 
the attitude of the vehicle and its height above the ground. The inputs are the 
estimated support plane and the plane formed by the bodys lateral and 
longitudinal axes. 
Body attitude regulation is accomplished by rotating the present body 


plane towards the desired body plane. The desired plane can be expressed as a 


function of the terrain slope and be adjusted to suit the driver’. 


The unit vector B, along the rotation axis. (Fig. 3.4) is given by 


ence ce : T 
a = =[k, kk, (3.21) 








where <, and Zp) are the unit normal vectors of the current and desired body 


* In the current level terrain model, the desired body plane angle is set equal to zero. 
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REPRODUCED AT GOVERNNENT EXPENSE 
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Figure 3.4 Rotation Axis and Angle 


planes and k_ = 0. The rotation angle, 4, is given by 


These values are used in the control function to obtain the rotation rates around 
the body’s longitudinal axis, W and about its transverse axis oe, 
Body altitude is defined as the distance along the body plane’s unit 


_ 


norma. rom the estimated support plane to the body's center of gravity. 
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A mapping function similar to that used for the body plane can be used to relate 


the desired altitude. h,. to the current terrain slope. De 


3. Rate Computation 





The differential equation describing the simulated dynamics of the control 


filter is 
ON) == (1) (3.23) 


where 7 is the time constant of motion and y(t) is difference between the desired 
and actual position variable. Integrating both sides of equation 3.10 yields an 


exponential response 


we (hy h). (3725) 


* In the current model the desired height is set to a constant value 


aul Cw _—~ 


oe ee a ee a ee cel ee 


The rotational vector +, decomposes into rotation vectors about the forward and 


lateral axes. vielding: 


} 
igh. Oe K 7 
i 
il 
a 
y u 
ie 


Velocity is related to acceleration using the same filter. 


accomplished by letting 


and substituting into equation 3.23, yielding: 


The accelerations for the remaining three rates are 


l 

73 oe (+3 commanded “7 noe an) 
Tn 

¥ es 

YR - —(Yp commanded YR ee 
i, 
1] 

ssh: - ~~ (4, commanded _ me here) 


Using the Irear approximation. 


A\velocity ~ Atime-acceleration, 


the rates are determined by equations 3.35 through 3.37, 
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(3.30) 


(3.34) 


any 


2p new (Ip commanded 7 wee, Ts, current (3.35) 
Ty 

; Bree. . 

YR new (Ye commanded YR a) = YR current (3.36) 
i 
it 

- new - (w, commanded ~“2z nee ~ “> current (oro) 


‘s 


where At is the time increment and 7, is the time constant of motion. 

Body positioning in this computer model is achieved by translating the 
body center to its proper earth coordinate position and then successively rotating 
the body about its vertical. transverse and longitudinal axes. In order to do this. 
body rates are first transformed into earth coordinate translation rates and body 


Euler angle rates using the method presented by Frank and McGhee [Ref. 19]. 


6 1 tan®sinO tan®cosO lee 
ci 0 cos sinO Ww, (3.38) 
u QO secOsinO sec®cosO 


Roll, elevation and azimuth angles and translation distances are then found 


through simple Euler integration: 


—— — 


Uae: y - Atime. 


Co 
ew) 
Ke) 
~~ 


Ynew 
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4. Leg Kinematics 
The ASV’s pantograph leg contruction vields relatively simple kinematic 
and inverse kinematic equations. These equations differ slightly from those 
presented by Lee /Ref. 4]. This can be attributed to the use of more accurate 
dimension measurement than those assumed by Lee. 
For the front left leg (leg number one) shown in (Fig. 3.1 and 3.2), the 


foot position is given by 


r,= od, +h, (3.40) 
Yr [Ola sims cose hy (3.41) 
s, ~ UsinO ~ (5/, 4d,)cos© (3.42) 


where the hip position (h,. h,. h,) and foot position (r,. Yy- =r) are given in body 
coordinates. and d,. d,. and © are the joint variables. 
The inverse kinematic equations for the joint variable d,. derived from 


equation 3.40 1s 


de (x, ane (3.43) 


a”sin°® — a | sinOcosO 4 I cos © = (y, - h,)” (3.44) 
i sin" O~ Gein cos O ee a"cos’© = (=, — h.)° (3.45) 
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where a (5/, + 4d,). Solving equations 3.31 and 3.32 gives. 





In addition to the joint parameters, this model requires leg upper (thigh) 
angle. a. the lower leg (shank) angle. +. and the knee position in body coordinates 


f 


(z,. y,- 2,). The thigh angle is given in terms of joint variables as 


OQ 


ge | 3 


=a ] 
ecilcld SEs — €O5 et a (3.48) 





and the knee position as 


eC Onc ii (3.49) 
y, ~ (sine d,)sin© + l,cosO + h, (3.50) 
z, = l,sin® — (/,sina — d,)cos© (8251) 


The knee angle is 


All six legs of the ASV share similar geometries. The remaining 
kinematic and inverse kinematic equations can be obtained from equations 3.40 


through 3.52 with appropriate sign changes. 
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EF. SIWULA TIONS ILITIES 
1. Hardware 

The computer simulation presented here is designed to run on either of 
the two Siheon Graphics. Inc. IRIS-2400 workstations currently in the computer 
graphics laboratory in the Department of Computer Science at the Naval 
Postgraduate School. The IRIS (Integrated Raster Imaging System) consists of a 
Geometry Pipeline. a general purpose microprocessor. a raster subsystem. a 60Hz 
non-interlaced high-resolution RGB display monitor and a kevboard. In addition 
each unit has been equipped with two 72 megabyte disk drives, a cartridge tape 
unit. a floating point accelerator. and a three-input mouse. The Geometry 
Pipeline is a series of ten or twelve custom VLSI chip matrix multipliers. Under 
the control of the applications graphics processor. it performs matrix 
transforinations. clipping and scaling of coordinates. The output is sent to the 
raster subsystem which performs functions such as filling in pixels. shading, 
depth-cueing and hidden surtace removal. 

The first IRIS system is based on a Motorola MC68010 processor with 5 
megabytes of CPU memory and a 1024 x 786 x 8 bit display memory. It 1s also 
equipped with a digitizer tablet. The second IRIS system is a more capable 
Turbo-2400. It is based on a Motorola MC68020 processor and has 4 megabytes 
of CPU memory and a 1024 x 768 x 32 bit display memory. An Ethernet network 


connects both workstations to two VAX 11/780’s and one VAX 11/750. 


o0 


2. soltware 
The IRIS Graphics Library contains a large number of graphics 
commands and utilities. This allows the user great flexibility in the choice of 
coordinate systems and display techniques. While the software is written in C. 
the graphics commands may be called in C. FORTRAN, Pascal, and Lisp. The 


code for the model presented in this study is written exclusively in C. 


+ 


ee OUMNIAR Y 

The previous sections of this chapter outlined the physical constraints. 
simplifications. and tools used in the development of this simulation. The next 
chapter describes the operation and construction of the actual simulation 


program. 
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IV. SIMULATION PROGRAM 





A. INTRODUCTION 

In this chapter the simulation program is presented. The first section consists 
of a user’s guide, with complete instructions on how to use each program feature. 
The second section introduces the working environment for graphics on the IRIS- 
2400. The final section describes the internal operation of the simulation program 
and discusses the flow through the major modules. A complete listing of the 


program is provided in the appendix. 


ie Stok Ss. Caeekin 


1. Starting Up 





The program walk.c is relatively simple to use. It is entirely menu-driven, 
with a single mouse button and cursor performing all selection functions. To start 
the program. type the command "walk". 

Immediately displayed on the monitor is a split screen view of the control 
panel and the ASV on its terrain (Fig. 4.1). The right half of the screen features 
a three-dimensional projection of the ASV on a green and white checkerboard 
plane against a blue backdrop. The user's vantage point is fixed relative to the 
center of gravity of the vehicle (above and initially to the vehicle’s left side), so 


that the vehicle will continuously remain in view while walking. 
a2 
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The left half of the screen contains a two-dimensional representation of 
the control panel. Initially it features only the six vellow selection panels of the 
main meuu on a cyan background. 

2. © Nifemnis 

A menu item is selected by placing the cursor over the corresponding 
panel and clicking the middle mouse button. Pressing the button down will cause 
the panel beneath the cursor to be highlighted in red. as a potential choice. 
Releasing the button selects the highlighted menu item. If no changes are desired 
in the current menu selection. simply move the cursor to a portion of the screen 
outside the menu selection region and release the mouse button. Selected menu 
iterns are highlighted in bright vellow. 

3. Forward Wave Gait 

In the forward wave gait mode, vehicle velocities are specified in terms of 
body axis translation and rotation rates. Three of these rates - longitudinal 
velocity, lateral velocity. and yaw rate, are directly controllable by the operator. 
The rates for the remaining three degrees of freedom are automatically adjusted 
by the vehicle in order to maintain proper attitude and altitude. All rates in this 
mode are defined with respect to the body s center of gravity. 

Selecting the forward wave gait panel produces a secondary menu 
displayed immediately below the main menu (Fig. 4.2). This secondary menu 


contains three additional panels for use in specifying the vehicle's body rates. The 


panels are operated in the same manner as those in the main menu. To the right 
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of the menu panels are six simulated LED readouts. used for displaying the 
magnitude of the current and ordered rates. 

Releasing the middle mouse button while the cursor is inside the bounds 
of one of the secondary menu panels results in a sliding bar control panel being 
displayed on the left edge of the screen (Fig. 4.3). Velocity commands are input 
by placing the cursor within the black center region of the bar control area. A 
vellow bar level indicator will rise or fall to match the cursor level. indicating the 
commanded velocity value. No clicking of the mouse button is required. To set 
the commanded input at the desired level, move the cursor to the desired height 
and then slide the cursor horizontally until it is outside the center region of the 
sliding bar panel. A red bar level indicator displays the current velocity of the 
vehicle. 

4. Follow-the-Leader Gait 

Control while in the follow-the-leader gait mode is achieved by specifying 
the desired relative velocity vector of the ASV's steering point. The operator, in 
essence, points the vector in the direction in which the steering point should 
travel. relative to the body longitudinal axis. As stated in the previous chapter, 
the steering is very much like that of a long tractor-trailer type of vehicle. The 
control algorithm factors in the magnitude of the desired velocity, footholds and 


~~ 


current velocity and automatically regulates the body's motion. 
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The follow-the-leader gait control mode is invoked by using the lower left 
panel of the main menu. When this panel is selected. a white rectangular control 
area appears directly beneath the main menu (Fig. 4.4). If the middle mouse 
button is held down while the cursor is within this region. the cursor controls a 
simulated two-axis joystick. The vertical axis represents the magnitude of the 
relative velocity vector and the horizontal axis represents the direction. A solid 
yellow line is used to indicate the current joystick position, and thus the input 
values. The vehicle's actual relative cockpit velocity vector is indicated by a solid 
red line. 


5. Status and Warnings 





The status menu option exists to provide the operator with numerical 
data on leg and body position and movements. Selecting this item causes a 
vellow and black display panel to appear below the main menu (Fig. 4.5). 
Featured on this panel are the translation and rotation rates (with respect to the 
body axes). the position of the vehicle’s center of gravity (in Earth coordinates). 
the vehicle’s orientation (in Euler angles), the walking cycle period, the position of 
each foot (in body coordinates) and the angles of various components of the legs. 
The values are updated each display cycle. 

During the operation of the vehicle, checks are made on operating 
parameters. If a leg become positioned so that the foot is outside its 


corresponding constrained working volume. a red warning box is flashed in the 


lower left corner of the screen. Similarly, if the walking cycle period becomes too 
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Figure 4.4 View of Follow-the-Leader Gait Screen 
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small. a vellow warning box is displayed. In addition to the warning. a 
deceleration routine is activated to slow the vehicle until the period comes up to 
an acceptable level {Ref. 4: pp. 66-71]. 
6. Reset and Exit 
The reset option returns all vehicle parameters, including position. to 
their original values. This feature was included to save time when making a series 
of test runs. The exit option ends the program. clears the screen and returns the 


user to the current UNIX shell. 


eee GRAPHICS ON THE IRIS-2400 

Figures are displayed on the IRIS-2400 by calling a series of short graphics 
commands. called primatives. The primatives are interpreted into graphical 
displays by the software and special hardware of the IRIS system. These include 
commands for specifving color. drawing lines. circles, irregular polygons. and 
printing text characters on the screen. There are also a series of primatives 
designed to manipulate coordinate transformation matrices for the purpose of 
scaling. rotating and translating figures. 

A sequence of graphics commands may be grouped into a listing called an 
object. This object list may then be conveniently executed using a single call. 
Once created. the object list may at any time be edited as desired through the use 
of object tags. The object, in essence. functions as a reconfigurable graphics 


subroutine. 
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Objects and figures in this program are displayed in the reverse order in 
which they are called. The last object is overlaid in front (with respect to the 
viewer s Vantage point). of the previously called objects. An important exception 
to this is the treatment of the back face of polygons. The reverse side of a 
polygon, in the back face polygon removal mode. is considered transparent and is 
automatically removed from the image as a hidden surface by the IRIS hardware. 
This feature enables the display of more realistic appearing three-dimensional 
objects. 

In the double buffer display mode utilized by this program. the special display 
memory is divided into two sets of bit plane buffers. As one buffer is having 
display data written into it, the other is used to refresh the monitor. Once the 
writing is complete, the functions of the buffers are swapped. and a new cycle of 
writing commences. This display mode provides for a smooth simultaneous 


update of the entire screen. 


D. PROGRAM ORGANIZATION 
The simulation program can be divided into three general sections: 
initialization. simulation loop. and termination. The heart of the program, the 


simulation loop. cycles through an input phase, which serves as the operators 


_ 


control interface for the vehicle: the calculation phase, in which the parameters 
for the position and orientation of the ASVW’s body and legs are determined; and a 


display phase. The initialization section performs tasks, such as defining 
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coordinate systerns and creating object lists. required to start up the loop. The 
termination section clears the screen and buffers in preparation for the next IRIS 
user. A flow chart featuring the program's primary modules is shown in Figure 
4.6. 

Since the order in which objects are called is critical in this display mode, 
special provisions are needed to create a full 360 degree viewing coverage of the 
maneuvering vehicle. Specifically, four separate ASV objects lists are created in 
the object construction module of the initialization section. Each object has the 
vehicle components ordered for proper viewing from one of four viewing 
quadrants. The display section therefore needs only to determine from which 
quadrant the vehicle is to be viewed and call the appropriate object. 

The simulation loop is the dominant part of the code. containing the 
overhelming majority of the program modules. The loop begins with a call to the 
drivers command interface module. This module controls the operation and 
display of the menu system, the status panel, and the sliding bar and joystick 
controls, as well as processing F.T.L. gait steering commands. The organization 


of the control module is shown in Figures 4.7 and 4.8. 
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Figure 4.6 Main Simulation Flowchart 
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Figure 4.7 Command Interface Module (1 of 2) 
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Immediately following the command module are two checks on the program 
status. If the exit option was selected in the command module, the loop is 
interrupted and the program enters the termination stage. The reset option 
causes a module call to re-initialize all working parameters. 

The calculation phase of the loop is begins with the support module. where a 
determination of the estimated support plane directly beneath the vehicle is 
made. The position and velocity of the ASV’s body is then calculated in the 
body rates module (Figs. 4.9 and 4.10). The body kinematics used in this module 
are discussed in section III.E. 

The gait period is next calculated in the optirmal period module. This module 
uses a optimal period contro] algorithm which considers the kinematic limit of the 
supporting legs. In this algorithm, a period is calculated for each leg. based on 
the time required for its foot to reach the limits of its corresponding constrained 
working volume. The minimum of all of the supporting legs periods is chosen as 
the vehicle's optimal period. No foot should therefore be required leave its 
constrained working volume while supporting the body. A backward gait period 
is also computed for the use of the wave gait walking in the reverse direction. 


[Ref. 4: pp. 63-69] 
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Figure 4.9 Body Rates Module (1 of 2) 
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Figure 4.10 Body Rates Module (2 of 2) 
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The deceleration module checks the output of the previous mnodule. If the 
period is below the set threshold, the vehicle 1s slowed. Each time the period falls 
below the minimum value. longitudinal body velocity is cut ten percent and 
lateral velocity and yaw rate are cut twenty percent. This slowing occurs in each 
pass until the walking period rises to acceptable limits. 

The leg phase module is used to update the movement phase of each leg. 
The phase. expressed as a modulo one floating point number, indicates at what 
point the leg is in its cycle of supporting. lifting off from the ground, being 
transferred toward the desired foothold. and being placed onto the ground. The 
relative phases of the legs in this simulation are set to move the legs in two, 180 
degrees out of phase tripods. 

The foot trajectory module uses the leg phase information and the period in 
calculating the position of the feet relative to the body. The algorithm is shown 
in the flow chart of Figure 4.11. The transfer time is the length of time allotted 
for moving the foot from one foothold to the next. This determines the speed in 
which the transfer is made. 

The phase of the leg relative to the beginning of foot liftoff is referred to as 
the transfer phase. When the leg’s transfer phase is negative. corresponding to 
being on the ground in a supporting role. the foot’s relative position is determined 
by the support trajectory module (Fig. 4.12). When the leg’s transfer phase is 
greater than zero but less than the liftoff-transfer transition value, the relative 


foot position is returned by the liftoff trajectory module (Fig. 4.13). Likewise a 
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Figure 4.11 Foot Trajectory Module 
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transfer phase value between the two transition point values yields a response 
from the transfer trajectory module (Fig. 4.14 and 4.15). and a phase value 
greater than the transfer-placement transition value yields a relative foot position 
calculation from the placement trajectory module (Fig. 4.16). 

The foothold selection algorithms contained in the transfer trajectory module 
are discussed in section III.C.3. Note that within this module the desired end foot 
position is treated differently in the follow-the-leader and forward wave gait 
modes. The forward wave gait. with its high degree of maneuverability, has a 
considerable greater probability that the projected ideal position toward the end 
of the transfer phase will be much different from that at the start. Therefore the 
desired foot position in the case of the forward wave gait is updated on each pass. 
In the follow-the-leader gait case it is only calculated during the first time through 
the the module. 

The results of the calculation phase are the position and orientation of the 
body and the relative position of each of the feet. These values are used. with the 
inverse kinematic relations derived in section III.E.4, in the display phase to 
obtain the rotation angles and translation distances required to position the 


ASW’s component parts. 
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Figure 4.14 Transfer Trajectory Module (1 or 2) 
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The ASV object lists are then edited and the updated parameters inserted 
into their corresponding rotation and translation commands. Once this is 
completed, the display calls are made for the background. the terrain object and 
then the properly ordered ASV object. Swapping display buffers completes the 
loop. 

The ASV simulation prograrn presented here consists of fifteen separate files 
linked. together with the graphics. math, and standard input-output libraries, 
using the UNIX make utility. The program files and Makefile listings are 
presented in the appendix. The routines were created in a modular fashion for 
ease of development and testing and to assist in future program changes. 
Constants are grouped into a single header file walk.h, for convenient reference 


and modification. 


Ek. SUMMARY 

This chapter describes the ASV simulation program. The first section is a 
guide for the operation of the program. It details the use of the menu system and 
the operator controls. The following section discusses the display of graphics on 
the IRIS-2400. The final section covers the organization and flow of the main 


routine and its modules. 


lle ae 
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V. SIMULATION PERFORMANCE 





This chapter provides a brief review of the performance of the ASV 
simulation program. The review is largely subjective and is based on the author's 


experience with the operation of the simulation. 


A. MODELING FIDELITY 

The image of the vehicle on the screen appears to be a reasonable likeness of 
the actual ASV. This is believed, to a great extent, to be due to the proper 
scaling of dimensions of component parts. based on available blueprints of the 
ASV. Details such as the leg hydraulic actuator housings and the optical 
scanning radar. mounted on the cab of the vehicle. add to the visual effect. The 
color scheme of the simulation vehicle has been altered to enhance the visibility of 
the vehicle and its parts. 

The walking motion of the model is very similar to that of the real vehicle. 
This observation is based on viewing of videotapes produced at the Ohio State 
University. A notable difference is that the simulation model is perceived to 
operate at a much slower speed. A simulation time increment of 1/100th of a 
second vields a display time to real time speed ratio of 1:30. Operating the 
simulation with a simulation time increment much greater than 1/100th of a 


second to compensate for this causes problems related to the optimum period and 
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leg phase modules of the program. This leads to gross errors in the foot trajectory 


planning algorithms. 


B. FORWARD WAVE TRIPOD GAIT 

Driving in the forward wave tripod gait mode is a very simple task. Although 
a three-axis joystick would be prefered. the mouse-driven sliding bar control is 
easy to use and effective. Switching between control bars for forward, lateral. or 
rotational control can be accomplished with reasonable ease. 

The externa! vantage point of the vehicle causes very little problem for 
inaneuvering the vehicle. This may change as the model’s speed increases and 
obstacles are introduced into the environment. 

Overall. maneuverability of the ASV in the forward wave tripod gait mode is 


clearly demonstrated with this model. 


C. FOLLOW-ITHE-LEADER TRIPOD Gait 

The follow-the-leader tripod gait appears to work especially well for forward, 
straight-line locomotion. Turning. however. is extremely restricted. Preliminary 
investigations indicate a mininum turning radius of 18 times the body length. 
using the constrained working volumes depicted in Figure 2.4. This is far greater 
than expected. An estimated envelope for turning, based on initial simulation 
trials. is shown in Figure 5.1. Steering commands falling outside of this envelope 


result in faults within the foot trajectory planning algorithms. These faults 
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usually occur within the first four degrees of the turn atteinpt. Decreasing the 
display time interval extends the maximum magnitude of the command envelope. 
but only marginally improves the permitted relative direction command input. 

The shape of the steering envelope is rather unexpected. and as of vet. 
unexplained. Factors likely to have the greatest influence on the envelope are the 
geoinetry of the legs constrained working volume and the implementation of the 
optimum period and foot trajectory planning algorithms. 

Expanding the constrained working volume to the full working volume has a 
remarkable effect on the maneuverability of the vehicle. while operating in the 
follow-the-leader gait mode. By doing so, the minimum turning radius improves 
to approximately five times the body length. This indicates a great potential 
advantage in utilizing dynamic stability algorithms for future gaits. 

Overall the follow-the-leader gait and tractor-trailer steering appear to be 
successful for level, relatively obstruction-free terrain. Further research is needed 
to determine the nature of the limitations and the means to expand the vehicle's 


maneuverability while operating in this mode. 


VI. SUMMARY AND CONCLUSIONS 


In this thesis a tripod follow-the-leader gait class is introduced for use by six- 
legged walking vehicles. The class represents an extension of previously defined 
follow-the-leader gaits and should prove useful for legged vehicles traveling in 
rough or treacherous terrain conditions. 

A new stvle of steering is also developed for follow-the-leader gaits. This 
steering mode exhibits a general response similar to that found in steering a 
wheeled tractor-trailer vehicle. With this mode, the driver is concerned only with 
specifving the velocity of the front of the vehicle. The algorithm ensures that the 
body of the vehicle follows along the path of the front. 

An improved simulation model constructed to study the gait and steering 
algorithms is also presented in this thesis. The vehicle selected as a physical 
reference for the model is the Adaptive Suspension Vehicle (ASV). which is 
currently undergoing testing and development at the Ohio State University. The 
model developed is intended as a general tool for analyzing a variety of walking 


control algorithms for legged vehicles. 
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A. RESEARCH CON PRIBC TOM. 

Previous research on follow-the-leader gaits (Ref. 3). has concentrated on gaits 
that are temporally oriented. Since footholds are used by the following legs 
immediately after being abandoned by the lead leg. this produces a creeping 
motion with alternating leg and body movement. 

Extending the class of follow-the-leader gaits into the spatial domain relieves 
the requirement of immediately utilizing a foothold as soon as it is abandoned. 
This gives a greater degree of freedom to leg movement and allows the possibility 
of smooth. continuous body motion with shorter leg stroke. 

The nature of a follow-the-leader gait greatly constrains the maneuverability 
of the walking vehicle. The vertical projection of the vehicle's center of gravity is 
required to fall within the support pattern of the legs and is therefore confined by 
the history of footholds produced by the lead legs. The similarity of this problem 
to that of a trailer pulled by a tractor cab has inspired the adoption of the term 
"tractor-trailer" steering. With tractor-trailer style steering. the driver controls 
the path of the front of the vehicle. As long as the driver does not turn too 
sharply (possibly causing a wheeled tractor-trailer to jack-knife). the vehicle's 
body follows along this path. 

The selection of footholds for the leading legs is based on projecting the 
relative heading vector provided by the operator. The location of recently 
abandoned footholds is retained within the control algorithm for use by the 


middle and rear legs. 
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The simulation presented in this study models the kinematics of the ASV. 
The model incorporates many of the simulation features presented by Lee [Ref. 4]. 
including omnidirectional control. automatic body altitude and = attitude 
regulation. leg motion planning. body deceleration. and filters between the control 
inputs and reaction which provides the operator with the "feel" of vehicle 
dynamics. A simplified variation of constrained working volumes is also used. 

The simulation program has a modular design which creates a flexible 
environment for studying various gaits and control algorithms. The program as 
currently configured has two modes of operation. The first features a forward 
wave tripod gait with three-axis control for steering in body coordinates. The 
second mode utilizes the follow-the-leader tripod gait and two-axis control 
tractor-trailer style steering. developed in this study. The program’s displays and 
controls are operated with a mouse-driven menu package using a single mouse 
button. 

The graphics presentation is greatly improved over Lees monochrome line- 
drawing representation. Three-dimensional, solid body, color graphics are made 
possible through the implementation of the model on the special purpose software 
and hardware of the IRIS-2400 system. This provides a notable enhancement of 


realism for the vehicle simulation. 


—_—_— 
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B. RESEARCH EXTENSIC. = 

It has become clear. through the work of developing this study. that there are 
many directions in which future research could be pursued. Four major areas to 
be considered for extension are: quantitative Ineasurement of the FTL tripod gait 
performance. l1mprovemnent of program features. improvement of display speed, 
and expansion of upper level control algorithms using artificial intelligence. 

Developing performance criteria for the simulated ASV is critical if one is to 
effectively use the program as an aid for developing and evaluating walking 
algorithms for the actual machine. Initial research might well concentrate on 
Ineasuring turning radii, steering reaction times. stability margins and _ basic 
parameters of mobility. 

As with any simulation model. there are many desired features which could 
be added to enhance realisin. Perhaps the most important improvement for this 
type of mobile vehicle would be the inclusion of rough or uneven terrain into the 
model. Provisions were made in the development of this model for that 
eventuality. A few new algorithins, for functions such as estimating the support 
plane beneath the vehicle and adjusting the constrained working volume to ~ 
conform to the terrain slope, will need to be written. It should be possible to 
follow the work of Lee (Ref. 4], at least initially, in improving the simulation in 
this direction. 

Because the ASV is designed for rough terrain locomotion. developing a good 


foothold search algorithm is important. In addition, the inclusion of foothold 
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search into the simulation model would enable the FTL gait to be better 
evaluated with respect to other types of gaits in various terrain conditions. This 
would quantify the advantages of reduced foothold probing requirements for the 
eb eait. 

This simulation could also be used to further develop steering mechanisms for 
the ASV. Most notably, the algorithm for the tractor-trailer style steering uses a 
simple method for body positioning based on the centroid of the established 
footholds. A different method for body steering which minimizes the maximum 
leg excursions might improve the vehicle's turning radius. 

Dynamic modeling and supplementing the model's kinematics would greatly 
improve the realism of vehicle movement. Moreover, it should also notably 
increase mobility. as the vehicle would be free to utilize its leg’s full working 
volumes. This should also lead towards the development of a great number of 
new gaits, which are dynamically, but not statically, stable. 

Graphics techniques can be improved to enhance the realism of the displayed 
image. Features such as shading, depth-cueing. reflectivity of surfaces, terrain 
definition, and increased vehicle detail are all possible using current state-of-the- 
art techniques. Higher resolution monitors and an enlarged number of bit planes 
in the display hardware are also highly desirable. 

Adding additional features to the model has the decided disadvantage of 
requiring more cpu time for the simulation. As the program now exists, the 


simulated vehicle moves and reacts markedly slower than the actual vehicle. 
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There are. however. ways to improve the display time for the model. The prime 
means is of course to upgrade the hardware. using newly developed and more 
capable machines. The code may also be streamlined for efficiency. Possibly 
several of the interactive features could be reduced or eliminated. 

Another possibility for improving display time is the replacement of the 
integration routines within the body kinematics section of the program with an 
incremental homogeneous transformation niatrix technique used by Lee [Ref. 4]. 
Integration is used here because of the simplicity of the technique and the 
author’s familiarity with the IRIS-2400 special hardware commands for rotation 
and translation. It may be that the homogeneous transformation matrix could 
also be used directly with the special hardware to provide the full transformation 
with fewer trigonometric computations. This possibility has not been investigated 
by the author. 

An interesting avenue of research to explore is to automate the upper levels of 
the contro] hierarchy. It may be possible to use an expert system shell running on 
a special purpose LISP machine to provide driving commands to this simulation. 
As of this writing, efforts are underway by others to establish communications 
between the IRIS-2400 system and a Symbolics 3675 LISP machine. 


Extensions to the work presented in this thesis are possible and will likely 


~~. 


prove very fruitful. It is hoped that this line of research will lead to more efficient 
and practical gaits and control algorithms for legged walking vehicles in rough 


terrain. 
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APPENDIX 
PROGRAM LISTING 
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This program is written for the iris- 2400 
walk.c 

This is the main program for the simulation. 

Relle Lyman O04 May 1987 


AR KR RK OK KR ee OK KK OK KOK oe a EO oe eK ek ek oe kok ek ke oes ok 


#include "gl.h" 
#include "'device.h" 
include "walk h” 
#include <stdio.h> 
#include < math.h> 


main(} 


\ 
Object machineobject 4 leg 7 4 .textobj,vertextob) ,thighobj/7 2]/4,, 
actuatorobj 7 2/4 ,shinobj 7 2.4, walker 4:,groundobject: 


* NOTE: this program uses only elements 1-6 of arrays and vectors. 
Legs are nuimbered to remain consistent with original research. *- 


Tag transrot tag 4,,tr end tag 4. legmovetag 7 (4. 
actmovetag 7 2 4\.bodytagli4., 
thighmovetag 7 2 4 shinmovetag 7, 2/4): 


Colorindex wmask : 
int 1). kn. 


program status. ;* desired status of program: RUN, HALT or RESET * 
selected gait. .*~ indicates which tripod gait is to be used */ 


slow flag. _” flag indicating deceleration is needed *,; 
warning, * flag indicating supporting leg is outside of working volume */ 
leg status 7; * status of leg (supporting, liftoff, transfer, placement) */ 


static oat 
hxy7!= {0.155.,155.,0.,0.,-155.,-155.}, /* Leg attachment points *) 
hy 7 = {0,50.,-50.,50.,-50..50. 50.4, 
ha|7)= {0.23..23.,23. 23.,23.,23.}. 
l47 = {0,L4.-L4.L4.-L4,L4,-L4}; 


static Angle theta 7:'-{0,0.0,0,0,0,0}. , * Leg component angles * 
alpha:7, = {36 er 4,364 364 ,364,-364,-364}, 
gamma(t={3 17,317,317,317o)a- el io ode 


x * 


walk.c 
float temp.templ.temp2.temp3,top,bottom, ,* Temporary variables * 
alpharad 7 . .* Leg component angles in radians * / 
thetarad. 


legcoord x S, 


fi ‘* Foot position in leg coordinates *, 
legcoord y 7, 
legcoord 27 , 
azimuth,elev,roll, /* Body Euler angles {rads) *, 

ordered vel mag, /*~ Ordered velocity of the cockpit (magnitude) * 


ordered vel dir, /* Ordered velocity of the cockpit (direction) */ 


all 7 /* Joint variables * / 

2.7}. 

knee|7|/2., /* Relative position of knee * / 

foot |7|/2], /* Relative position of foot */ 

h|4]{4], /* Homogeneous transformation matrix */ 
invh|4}|4,, | /* Inverse homogeneous transformation matrix */ 
legphase 7,  /* Phase of individual legs * 

rel legphase7,, | * Phase of individual legs relative to leg one®* / 
period, /* Period of Jeg cycle */ 

min period, “* Minimum allowed period * / 

Ux ty.tz: -* Earth coordinates of body position * ’ 


vector rot rate. ‘* Body rotation rates */ 
trans rate, /* Body translation rates 
ordered rate, /* Ordered lateral and longitudinal translation and yaw rates 


ea j 


Bs 


footpos|7,, »* Position of foot in earth coordinates * / 
b footpos|7), /* Position of foot in body coordinates * / 
fh 7), /* selected footholds {earth coordinates) */ 
oldfh 7 ; /* old selected footholds (earth coordinates) */ 
work vol cwv|7|; '* Constrained working volumes * 
plane spe; /* Estimated support plane */ 


‘* Initialize the IRIS graphics * / 
ginit() ; /* standard IRIS graphics initialization * / 
doublebuffer() ; /* double buffering mode */ 


/ * 


gconfig() ; configure the IRIS (use the above commands * / 


wmask=(1<<getplanes())-1 ; /* enable all the bit planes for writing */ 


/* set to 2**(getplanes{))minus one 
/* all bit planes on ay 
writemask{wmask) ; 
backface(TRUE); ‘* set backface polygon removal on *, 
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Walle 


qdevice(MIDDLEMOUSE); ‘* set up the queue for the menu * 
tie( MIDDLEMOUSE MOUSE N MOUS iar 


mapcolor( LT YELLOW ,225,225.0): ‘* create new colors ~ / 
mapcolor( WHITE1.230,230,230}); 


view port(410,1023,0,767) ; /* set world view * 
perspective(600, (614.0, 768).0.0,1023.0) : 
make the ground *_ 
makeground(&groundobject); 
* make the robot , 
makewalker(machineobject.d1.d2,theta,knee,gamma.alpha,transrot tag, 
tr end tag,walker leg.thighobj.actuatorobj,shinobj, 7 
legmovetag,thighmovetag,act movetag,shinmovetag,tx.ty,tz,roll, 
elev.azimuth,hx.hy,hz,l4) ; 
Initialize the ASV walking routine parameters. *. 
initialize(hanvh.&rot rate.&trans rate,&ordered rate,&spe.& period, 
leg status.legphase,rel legphase, footpos,b footpos,ewv fh, 
oldfh.&selected gait,&ordered vel mag.&ordered vel dir. 
&min period.&program status,&tx,&ty,&tz,&roll &elev.& azimuth); 


while(TRUE) * Main program loop *, 
Input the drivers commands. * / 
driver command(ordered rate,&rot rate.&trans rate.&:program status, 
b footpos.& period.alpha.gamma,theta.&slow flag,&roll,&elev, 
fazimuth &ctx.&ty.&tz.&ordered vel mag,&ordered vel dir,fh, 
&selected gait); a - 


if (program status == HALT) 
{ 

/* Quit program. *,/ 

break; 


} 
if (program status == RESET) 


‘* Reinitialize the ASV walking parameters. */ 

imtialize(hanvh,&rot rate,&trans rate,&ordered rate,&spe,& period, 
leg status.legphase,rel legphase, footpos,b footpos,cwv,fh. 
oldfh.&selected gait,&ordered vel mag,&ordered vel dir, 


&min period,& program “status, &tx &ty &tz, &roll &elev,& azimuth Ji 
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* 


walk.c 


* Calculate the estimated support plane. ~/ 


_* Future revision needed for rough terrain. ”* 


support plane(&spe); 


-* Calculate the body rotation and translation rates. */ 

body rates(&rot rate.&trans rate,&spe,hinvh.&ordered rate, 
&tx, &ty,&tz,&roll &elev.&azimuth): 

* Calculate the constrained working volume for the legs. */ 

con work vol(cwv,b footpos,leg status,&warning); 


* Calculate the optimal period for walking. */ 
optimal period(legphase.b footpos.&rot rate,&trans rate,cwv, 

leg status.& period); 

* Decelerate if necessary. *— 
decelerate(&trans rate.&rot rate.&period.&slow flag,&min_ period); 
_* Calculate the phase of each leg. */ 
leg phase(legphase,rel legphase.& period); 


Calculate the new position for each foot. */ 
foot trajectory({legphase,&period,leg status,footpos,b footpos,fh,oldfh, 
invh,h.cwv,&trans rate,&rot rate.&selected gait); 


* 


Display the ASV on the screen. */ 


* This section computes the new parameters to position the legs 


relative to the body, based on the relative position of the feet. 
It then check to ensure that no actuator positions exceed the limits. */ 
* Convert foot position to leg coordinates. *, 
for(i=1; i<5; i+—) 
legcoord x{i] = b footpos il.x - hx|i]; 
legcoord y|i! = b footpos il.y - hy|il; 
legcoord z'i = b footpos|i .z - liz/il; 


} 


* The foot position of the rear legs are changed to compensate for 
the 180 degree rotation used in the leg construction routine. */ 
for(i=5; 1<7; i+ —) 
{ 
legcoord x/i) = hx/i - b footposii!.x: 
legcoord y|i = b footposlij.y - hy'i;; 
legcoord zi = b footpos!i .z - hz.il; 


} 
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«x 


walk.c 


for(i] lois pi} 
{ 

“ generate required parameters d1,d2, theta *. 
a2 in legcoord x'1 “5.0: 
temp= legcoord yii, * legcoord yiil!: 
temp2—legcoord 21 *legcoord 21; 
d1 i = -(5.0*L3-sqrt(temp+temp2-L4*L4)) /4.0; 
temp1=5.0*L3—4.0*d1{il ; 


switch (1) 
{ 
case |: 
case 3: 
case 5: temp3 = templ*legcoord yi + L4*legcoord 21); 
break: = 7 
case 2: 
case 4: 
case 6: temp3 = tempIl*legcoord yi) - L4*legcoord 2/1); 
} 


thetarad - asin(temp3 (templ*temp] + L4*L4)); 


thetahh = thetarad ~572 = 07s: 
for(i=l wi<7 ; i++) »* prepare parameters for graphics 
{ '* update on all 6 legs ae 


temp = L3+dIi ; 
templ = d2.i1 *d2 i) + temp*temp; 
temp2 = (LI-L1-L6°L6 2 temp)/(2.0° L1 sqrt(temp ie 


alpharad'1 =((PI/2)-atan(d21|/temp)-acos(temp2)) ; 


-* Note: One half of a degree has been added to all angles *, 
alpha i =(alpharad:i/*578+.5). 


knee 1..0'=(L2*cos(alpharad'1')—.5); /* relative to baseplate */ 

knee i t)= -((L2*sin(alpharad i|)- dti/)+0.5):/* relative to baseplate *, 
foot |iiOj= (5.0*%d2 i'+-.5); /* relative to baseplate */ 

foot|1) Li= -(5.0°L3+4.0*d1il+.5) ; /* relative to baseplate */ 
top=(knee.i 0/-foot 1|/0 ); 


bottom=(kneeli 1'-foot 1,1); 


gamma 1=(atan(top’ bottom)*573+.5) ; 
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for (n=O; n<4;n+-—) |* The walker is updated in each quadrant 
{ 


editobj(thighobj 1/0 \n') ; -* edit each leg to new 


1 


* 


ae i 


kK | 


objreplace(thighmovetagi 0 n!); /* location / 


fe 


rotate(alphali], Y’) ; 
closeobj() ; 


editobj(thighobj i]|1 .n) ; 
objreplace(thighmovetag)i) 1.in]) ; 
translate(0.0,.0.0,d11) ; 
closeob)() ; 


editobj(actuatorobj 1.0 n) ; 
objreplace(actmovetag 1|0 [n’) ; 
rotate(alphaiil,’Y’) ; 
closeobj() ; 


editobj(actuatorobj.i//1) nj) ; 
objreplace(actmovetagi 1 n') ; 
translate(d2 1 ,0.0,-L3) ; 
closeobj() ; 


editobj(shinobj i] O}/n ) ; 
objreplace(shinmovetag 1 On) ; 
rotate(gammai,’Y’) ; 
closeobj() ; 


editobj(shinobj/i) 1]{n}) ; 
objreplace(shinmovetag!i) 1|n)) ; 


translate((float)(knee|i!'0)),0.0,(float)knee i)/1)) ; 
closeobj() ; 


editobj(leg i /n)) : 
objreplace(legmovetag il/n|) ; 
rotate(thetai ,°X’) ; 
closeobj() ; 
} ’* end quadrant loop */ 


* / 


jay end for leg loop i=1 ... *, 


for (n=0; n<4; n++) 
{ 
editobj(machineobject n}) ; 
objdelete(transrot tagin|,tr end tagin ); 
objinsert(transrot_tagin]); jones 
translate(tx,ty,tz): 
rotate((int) (azimuth*573),°Z’); 
rotate((int}(elev*573),’Y"); 
rotate((int)(roll*573),’X’); 
closeobj() ; 
} /* end of quadrant loop */ 


oF 


| ie 


“set up the background * 
color(BLUE): 
clear(): 
“ Keep the viewing relationship constant. *— 
perspective(600,(614.0/768),0.0,1023.0) ; 
lookat(&00.0—tx,800.0+ ty .550.0,tx,ty,-50.0,1100): 


um CALL THE GROURD. | 
callobj(groundobject); 


“ Display the ASV in the correct quadrant configuration * 
if (azimuth <. 0.0) 
{ 
{ 
azimuth += 2.0 * PIT, 
: (azimuth = 2.02 Fl) 
asics 2=-2 0 Pi: 
: fazimuthisO.25 PI} 
: callobj(machineobject 0}): 
: (lazimuth >= 0.25" Plje:&(azimuth<10 75° FI) 
| callobj(machineobject 3)): 
f ((azimuth -= 0.75*PI)&&(azimuth < 1.25*PI)) 
| callobj(machineobject 2]): 
/ ((azimuth 9.1.25*PI)&& (azimuth < 1.75* Pi) 
| callobj(machineobject)1]): 
: (azimuth =— 19 F]) 
| callobj(machineobject 0]); 


swapbuffers() ; 


* end of main program loop *, 
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= walk.c 7 
* Clean up the screen. 


color(BLACK) ; 
clear{) ; 
swapbuffers(); 
color(BLACK): 
clear{): 
swapbuffers(): 
finish() : 

gexit() ; 


} /* END OF MAIN PROGRAM *, 
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yw eee ee ee ee Oe ar ee em ee ee ee ee ek oe oe ee ee ee 


This is the header file for the program walk.c. 
walk.h 
Relle Lyman 
i Nilay 1084 


FREE RE ES EE ER ee te ee eee 


#define BETA 0.5 
#define DELTA TIME 0.010 
#define TIME CONSTANT 1 0.1 
#define TIME CONSTANT 2 0.25 
#define TIME CONSTANT 30.5 
#define FTL GAIT 
+define FWD WAVE GAIT 2 
#define FORWARD ] 
+#define BACKWARD 0 
#define END LIFT PHASE 0.2 
«define BEGIN PLACE PHASE 0.8 


+ define SUPPORTING 0 

+#define LIFTOFF l 

#define TRANSFER FORWARD 2 

# define PLACEMENT ° 

#define ON ] 

#define OFF 0 

#define LENGTII 310.0 /* The length between the forward 
and aft hip joints */ 

#define HALFLENGTHI 155.0 ,* Half the length between the forward 


aa 


and aft hip joints 
#define FOOTLIFTHEIGHT 40.0 


#define LONG TIME 1000000 

#define HO 160.0 /* Desired body height (cm)*/ 
#define OUTER LIMIT 6.08. 7 > cm/sec; 
+#define INNER LIMIT 1.52 /* cm /see.a7 7, 
«define RUN 0 

#define HALT 1 

#define RESET 2 

#define NORMAL 0 

#define SLOW 1 

#define PI ola 159 

#define UP l 

+#define DOWN v 

# define IN 1 

#define OUT 0 

#detine LI YELLOW 100 

+define WHITE1 16a 

#define TEXTCOLOR BLACK 

#define NOHIGHLIGHT LTY ELEOW 


#define ACTIVEHIGHLIGHT RED 
#define INACTIVEHIGHLIGHT YELLOW 
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= walk.h * 


#define L1 20.0 
# define L2 102.0 
#define L3 24.0 
+#define L4 32.0 
+#define L6 30.0 


struct mag in xyz * 


{ 


tical X,¥,Z; 


ihe 


typedef struct mag in xyz vector; 


magnitude along x, y, and z axes */ 


struct plane coefficients /* plane coefficients * / 


{ 
f@at a.,b,¢.d: 
1 


typedef struct plane coefficients plane; 


ty pedef struct 
float min, 
max, 
center; 

\ dimensions; 


typedef struct 
dimensions x, 
y, 
Z; 
} work vol; 


typedef struct 


{ 


int left ,right,top,bottom,x0,y0.x1l,y1,x2,y2:; 
char *textO.*textl,*text2: 
\ menubox; 
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KK RK ROK KOK RO OK Ke OK KK KK KO KK OO OK OR Ke So ke oe ae fe Se ci  ee ee ee 


This is a function for the iris 2400 program walk.c. 
Init ae 


Relle Lyman 27 Apr 1987 


LUKE ENR KR AK RK KKK KK Kw RK EK EK KKK KK KR mR KK KOK OK FE EK OF KK KO oe oe ee eee oe 


#include "gl.h" 
#include "device.h" 
#include "walk.h" 


initialize(hinvh,rot rate.trans rate.ordered rate,spe,period.leg status, 
legphase.rel legphase.footpos.b footpos.cwv,fh,oldfh.selected gait, 


ordered vel mag.ordered vel dir,prograin status,tx,ty.tz.roll,elev,azimuth) 


* ‘This function computes the body rotation and translation rates. * 


vector ‘rot rate, ‘* rotation rate ~/ 
“trans akate, * translation rate * 
“ordered rate. ” ordered x translation, vy translation, 

and z rotation rates */ 

{hie * selected footholds (earth coord.) */ 
oldfh 7 . * old selected footholds (earth coord.) */ 
footpos 7, “ position of the foot in earth coord. * | 
b footpos 7; °* position of the foot in body coord. */ 


+ 


plane ‘spe: support plane in earth coord * 


work vol cwv'7,; -* constrained working volume * 


float hi4i}4 . * homogeneous transformation matrix * 
invh 44, * inverse of transformation matrix * 
legphase 7), /* phase of the phase */ 
rel legphase 7, »*~ phase of the leg relative to leg one * 
*period, »* body support period * ’ 

“x tye. ‘* position of body in earth coordinates * / 


*roll,*elev.*azimuth, /* body euler angles */ 
“ordered vel mag. * ordered velocity of the steering pt (magnitude) * 


“ordered vel dir, /* ordered velocity of the steering pt (direction) * / 
int leg status,7.. * status of the leg * 
“program status. “* desired status of program * 


ie. 3 


“selected gait: type of tripod gait to be used */ 


VL ab 


float modulus one(); /* modulus one function *- 
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Einit.c 


* initialize the transformation matrix * 


/ 








nO}LOg = 1.0; 
h'0| 1) = 0.0; 
h'0| 2. = 0.0: 
h/O|; 35 = 0.0; 
h 1|'0: = 0.0; 
eis = 1.0; 
nee zy = 0.0; 
illo — 0.0; 
m2 0! — 0.0; 
fez ialy = 0.0: 
h 2] Ue =a 1,0. 
h2|3, = HO; ,* initial height of the center of the body *: 
tou = 0.0: 
Milo lel = 0.0; 
hi3] 2| = 0.0: 
feoee ae 1.0; 


* initialize the inverse transformation matrix * 


forii=0; 1<3; 1++} 
{ 
for (j=0; j< 3; j++) 
| 
invhilij; = hj i; 
} 
me, 311, — 0.0; 
invhiii|3’ = -(h O}1)*h'O 
h{2][il*h/2)'3 ) 


J 


13) + ho lfis*hit!3 + 


} 

invh|3](3! = 1e0); 

/* initialize the body rotation and translation rates * 
Bouerate->x = 0.0: 

rot rate->y =e): 

rot rate->z = ONCE 

trans rate->x = 0.0; 

trans rate->y = 0.0; 

trans rate->z — 20) 


/* initialize the commanded body rates */ 


ordered rate->x = 0.0: /* translation *,; 


oo 


ordered rate->y = 0.0: ‘* translation */ 


ordered rate->z = 0.0; ;* rotation *, 


“period = LONG TIME; 
“selected gait = FWD WAVE GAIT; 
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- init-c 


* initialize the relative leg phase ~ 

rel legphase,l — 0.0; 

rel legphase 2) = 0.5: 

rel legphase\3 = BETA; 

rel legphase'4. = BETA-0.5; 

rel legphase!5 2 Bee. 

rel legphase 6 ~ modulus one(2*BETA - 0.5); 


* initialize the leg status and phase *- 
for (relist as 
| 
leg statusjii = SUPPORTING; 
legphase i ~ rel legphase 1: 


j 


* initialize the constrained working volume for each leg * 


ewv 1l.x.min = _ 95.0: 
CWraliscmax- <= 215.0. 
cwvelly center — 155.0; 
cws 1].¥.rnin 60.0: 
CWr | max | tole: 
CMs nen center — U5 0: 
CW 2 min = 2400. 
CW lez onax «=== = 8070: 
cwv'll.z.center = -160.0: 


Cw 2 )ex. iin) ee Joe: 

CW aomax = 215.0: 
cww 2mx.center ==. 155.0; 
cwv!l2i.y.min = -131.0: 
cwv 2|.¥.max = - 60.0; 
cwv|2).y.center = - 95.0; 
cwv 2!.z.min = -240.0: 
cw 22z.max .™=,- 80.0; 
cwwl2i.z.center = -160.0; 
cwv|3).x.min = - 60.0, 
CW Sie max = OU; 
cwv 3..x.center On0: 

cwsld..min &@ 60.0; 
cwv o .¥.max = 131.0; 
cwvl3].y.center = 95.0; 
cwv3iiz.min = -240.0; 
CW le) 2.inas y= OURO. 
cwv!3|.z.center = -160.0; 
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cwrd!. 
Cy 5: 


cwvi6).: 


cwv'6. 


cwv'6 


cwr'6, 
cwv 6: 


cwv'61y. 


cwvi6.. 
cwv 6. 


cwv 6). 


(Of 


1 pe 
Pig |X 


fhi3).. = cwv| 


fh 4|.x 
fh 5).x 
X 


fh 6). 


fon (i—1, 


{ 


fh 
fe 


] 


suave ie 


| 
|. 


N N N 


2 @ 


Ne 
ay 


— cwv 
= cwv|2| 


= ewv/4| 


_min 


min 
max 


center = 
min 
max 


SCenUcT 


amines = 


max 


.center = 


-_min 
_max 
5|.x.center 


min 
‘max 
‘center = 


max 


center = 


_min 
_max 


center 


min 


-max 


- 60.0: 
60.0; 
O20: 


=a lte 1.0: 


= 6000: 
Sse 


- 240.0; 
- 80.0; 
- 160.0; 


221.0; 


3 


= 00-0: 
== boo 0: 


60.0: 


131.0: 


95.0: 


om 2400. 
==). 


- 160.0; 


=o. 0: 
- 95.0; 


== loo-0- 


a 1s0: 


are 6,05 OF 


Genter — --95.0: 


Hin) (= 
max 
-center = 


| 





- 240.0: 
- &0.0; 
- 160.0; 


initialize the selected foothold positions * / 
I1j.x.center + LENGTH/ 12.0; 
.x.center - LENGTH /12.0: 
3:.x.center - LENGTH /12.0; 
.xx.center + LENGTH/12.0 


’ 


= cwv/5|.x.center + LENGTH/12.0: 


= cwv'6|.x.center - LENGTH/12.0; 


-y 
Z 


eS a= 


e) 
= cwVi)|.y.center; 


= ().(): 
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+ 


init.¢ 


* initialize the earth relative foot positions * 
for (i= bi< 71+) 

footpos.i.x = fh i}.x; 

footposii .y = fh il.y: 

footpos|i .z = fhiji'.z; 


* initialize the old selected foothold positions * 


for (i= iii.) 

{ 
oldfhgijixseth 1.x - LENG Th, 3.0; 
oldfh ij.y = cwvli'.y.center; 
oldfh:i).z =— Ue 


“ initialize the body relative foot positions * 
for {i= 151<7 51+ +) 
b footpos|il.x = cwv i x.center: 
b footpos|ij.y = cwv ig v.center: 
b footpos i).z = cwv i.z.center; 


“ initialize the estimated support plane * 


spe-za = 0.0; 
spe-@b — 0:0; 
spe->>c = 1.0: 
spe->d = 0.0; 


* initialize the ordered velocity of the steering point * 
“ordered vel mag = 0.0; 
“ordered vel dir = 0.0; 


* initialize the body attitude and position */ 
=EOll = OF. 


“lex e100: 
az7imutn— 0.0: 
“tx = O.0: 

*tv =-0.0; 

*tz == HO: 


“ initialize desired program status * 
*program status = RUN, 


* end of initialize * ’ 
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Set eee Secrest 6 Ke kk kee cma mt Rare cK oh OR wis ee OR eS ee ke Ke ce et oe oo oy oe 


This is a function for the iris 2400 program walk.c. 
driver.c 


Relle Lyman 13 May 1987 
eee SS se eee ee eee ee ee ee ee ee Tee eee Se ee Se Se Se eS SS ee eee ee eee eee eS 
#include "gl.h" 
#include ''device.h" 
#include "walk.h" 
zinclude <stdio.h> 
#include <math.h> 


menubox box!|)={ 
meee 0.0,0°0.0,0, "not", "used" “here”. 
ne e007 0.520, 120,561 120,597 120, 627 "GAIT" "WAVE" "FWD", 
DiG@eeeo. 610.525.220.567 ,220,597,220,627, Sa TI TUDE" "AND" "ALTITUDE", 
Petrol Gi Gen o,c20 907.020,097,5020. 627," "RESET" " ™, 
MONE 200.525,000. 120-422,120,452,120-8482 "CAIT" " "OPTL", 
POPU a2 5: 550,220,422 220.452 220482 (REPOR D'. ’ "STATUS". 
mors 2520 580.020.422 320,452,520, 462, PROGRAM" " MEXIT", 


ee e@eyitn: cel 20 2o0en20+270, 120,290, "REVERSE MPORWARD".""TRANSLATE". 
mnie Os. c07150,1 20,170, 120,190,120, 210 "RIGHT "LEFT" "TRANSLATE", 
mie ts. 10 120.90 120110 120 130% RIGHT" "LEFT" "ROTATE". 


Hod, 20023110,20.120,250.120,270.120,290." "1%." ", 
H0)200,230,150,120.170,120,190,120,210," "4", 
190,200,150, 70,120,90,120,110,120,130," 7," 1," "), 


driver command(ordered rate.rot rate.trans rate,program status, 
b _footpos, period, alpha,gamma. theta.slow flag,roll, elev, 
azimuth,tx.ty,tz.ordered vel mag.ordered vel dir,fh,selected gait) 
/* This function inputs the driver’s commands using a menu and 
the mouse. * 


vector *ordered rate, /* ordered x translation, y translation, and z rotation rates * / 
*rot rate. /* actual rotation rate vector */ 
*trans rate, _/* actual translation rate vector *, 
b footpos|7 . ,* position of foot in body coordinates */ 
hh} 7; /* selected footholds (in earth coordinates) */ 


int “program status. /* desired status of tins program RUN/HALT/RESET */ 
“selected gait. /* desired tripod gait * 


“slow flag: /* flag indicating eeeaon is required */ 
float * period, /* body support period ” | 
*tx.*ty,*tz, /* body translation distance (Earth coord) */ 


* 


azimuth, elev, *roll, /* body Euler angles 
“ordered vel mag, /* ordered cockpit velocity (magnitude) */ 
“ordered vel dir; /* ordered cockpit velocity (direction) */ 


107 


driver.c 


Angle alpha 7). “ thigh angle * 
gainma 7, * shin angle * 
theta 7; * leg lateral angle ~* 


Device dummy.x.y; 

static int buttonflag = UP, pick, potentialpick.mainmenupick,submenu,slidebar; 
int i 

oat barvalue: 

static float time: 


char str orx 100|,str ory!100!,str_orz 100), 
str trx 100 ,str try'100),str rrz 100 ,str time 100); 


pushmatrix(): 
pushviewport(); 


view port {0.500,0.767): 
ortho2(0.0,500.0.0.0.767.0): 

color(C YAN): * screen background color * / 
clear(); 


* Display simulation time on top of screen * 
color(TEXTCOLOR); 
time += DELTA TIME, 
sprintf(str time,"simulation time °C8.3f" time); 
cmov2i(105.700); 
charstr(str_ time): 


if (qtest() == MIDDLEMOUSE) ™* Button just pressed or released * 
\ 


qread(&dummy): 
qread(&x); 
qread(&y); 


if (buttonflag == DOWN)  /* Button was just released. * : 


\ 


butioniiag. = san 


if (potentialpick == 0) 


{ 


/* No change 7/ 


} 
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else if (potentialpick < 7) °* Main menu chosen 
{ 

mainmenupick = potentialpick; 

Bick ——0; 

pick = potentialpick; 

potentialpick = 0; 
} 


else 


pick = potentialpick; 


/* submenu chosen */ 


/* no change to main menu pick */ 
potentialpick = 0; 
j 
} 


else /* Button was just pressed. */ 


{ 
butteniag = DOW N: 


} 
\  * end of qtest *' 
if (buttonflag -- DOWN) —,* Find the box over which the 
cursor hes for highlighting. */ 
{ 
x = getvaluator(MOUSEX): 
v = getvaluator(MOUSEY): 


potentialpick = 0; 
fer (i=lci<7;1-+) /¢* Check the main menu. ~) 
{ 
if (x < boxiij.nght && x > boxfi left && 
y < boxi).top && y > box/ij).bottom) 
{ 
potentialpick = 1; 
} 
} 
if (submenu == 1) /* Check submenu #1. */ 
\ 
for (i=7:1< 10:1+ +) 
{ 
if (x < boxii|.right && x > boxiil left && 
y < box! top && y > box|/i).bottom) 
{ 
potentialpick = 1; 
j 
} 
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if (submenu == 2) /* Check submenu #1. * 
for(i POs toe) 
if (x < box i .right && x > box i|left && 
vy < box'i|.top && y > boxiil. bottom) 
potentialpick: = 
} 
j 
} 
else /* button is up */ 
potentialpick ~ 0: 


* Display the menu. * 


for (i= lai a 
{ 
if (1 == potentialpick) 


color(ACTIVEHIGHLIGHT); 


\ 
§ 


else if (1 ~~ mainmenupick) 


coler(I NACI EnIG HEIGHT): 


else 


color( NOHIGHLIGHT): 


\ 
J 


fe 


rectfi(box i left, box/i].bottom, box/i].right, box|i).top); 
color(TEXTCOLOR),; 

recti(box i).left, box/i).bottom, box i|.right, box i).top); 
cmov2i(box/i!.x0,box/i].y0); 

charstr(box|i|.text0); 

cmov2i(box i).x1,box,1).y 1); 

charstr(box(i].text1); 

cmov2i(box/i].x2,box}i].y2); 

charstr(box)i!.text2):; 
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ariven,¢ 
if (submenu 1) Pieciay suomenu + hn 
{ 
for(i1=75)= F0;1- + } 
{ 
if (i == potentialpick}) 
{ 
color(ACTIVEHIGHLIGHT); 


} 
else if (i == pick) 


color(INACTIVEHIGHLIGHT); 
} 


else 


{ 
color(NOHIGHLIGHT); 


rect fi(box'1 .Jeft, box 1,.bottom, box. .right, box 1.top); 
color(TEXTCOLOR); 

recti(box 1 Jeft. box 1 .bottom, box'i .right, box i).top): 
cmov 2i(box)1).x0,box 1, .y0); 

charstr(box/i .textO); 

cmov2i(boxli|.x1,box i].y1); 

charstr(box/i).textl); 

cmov2i(box |i .x2,box 1.2); 

charstr(box|i).text2); 


color{ WHITE); ‘* Draw LED gages. * 
rect fi(200,70,400,370); 
exior( BLACK); 
recti(200,70,300,150): 
recti(300,70,400,150): 
recti(200,150.300,230): 
rect1(300, 150,400,230); 
recti(200,230,300,310); 
recti(300,230,400,310); 
pecti( 200,410,300,370); 
recti(300,310,400,370); 
color(RED); 

cmov2i( 205,350); 
charstr("ORDERED"); 
cmov 2i{ 205,330); 
charstr("RATE"); 
cmov 21(305.350); 
charstr(" ACTUAL"); 
cmov21(305,330); 
charstr("RATE"); 


lil 


a 


if (submenu =— 2) * Display submenu #2. 


driver.c 


* Display the parameter values. * 
sprintf{(str_ orx,"[¢7.2f ordered rate- >x); 
sprintf(str ory. 807 2f" ordered rate- Se 


FAC Us 
sprintf(str orz. he ordered rate- a 


Sprinti(str tts, vane trans: rate->x 
Pp 

sprintf(str try. (007. 2f" trans rate- seis 
sprintf(str rrz,"%7.2f rot rate->z); 


emovz1(205.270), 
charstr(str orx); 
emov2i(205,190); 
charstr(str ory): 
cmov2i(205.110); 
charstr(str orz); 
cmov2i{305,270): 
charstr(str trx):; 
emov21(305.190); 
charstr(str try); 
cmov21{305.110); 
charstr(str rrz): 


5 


for (i= 101 lo | 
if (1 - = potentialpick) 
color(ACTIVEHIGHLIGHT), 
} 
else if (1 == pick) 


color((INACTIVEHIGHLIGHT), 


else 


{ 
color(NOHIGHLIGHT),; 


} 


rectfi(box i left, box/i bottom, box i .right, box 1.top): 
color(TEXTCOLOR); 

recti(box i!.left, box!i].bottom, box i .right, box i .top); 
emov2i(box 1).x0.box 1).v0); 

charstr(box 1,.text0); 

cmov2i(box!i,.x1,box!i].y1): 

charstr(box 1).text]); 

cmov2i(box.1 .x2,box:i].y2): 

Charstr| boxmutext2). 
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color(W HITE): * Draw LED gages. *, 
rectfi(200,70,400.370); 
colon(ioL CK): 
recti(200.70.300, 150); 
recti(300.70.400, 150); 
recti( 200, 150.300,230); 
recti(300. 150,400,230); 
recti(200.230.300,310): 
recti(300.230.400.3 10); 
recti(200.310.300,370); 
recti(300.310.400,370); 
color(RED); 
cmov2i(205,350); 
erarctr( ORDERED"): 
cmov 21(205,330); 
Gianstr( AING Li"): 
cmov21(305,350); 
charstr("ACTUAL"), 
cmov21(305 330): 


eharsth( ANGLE"): 
* Action! *, 


switch (pick) 


{ 


case 1: submenu — 1; 
*selected gait = FWD WAVE GAIT, 
break; 

case 2: submenu = 2; 
break; 

case 3: submenu = 3; 


“program status = RESET; 
break: 


case 4: submenu = 4; 
joystick(trans rate,rot rate,ordered vel mag.ordered vel dir,&buttonflag); 
steering conv(ordered rate,ordered vel mag.ordered vel dir, 
azimuth,tx,ty,fh); 
“selected gait = FTL GAIT, 
break; 
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Case 2. submenu = 5: 
status report(ordered rate.trans rate,rot rate, 
b foot pos. period.alpha,gamma.theta. 
slow flag.roll.elev,azimuth,tx.ty. 
C2) 
break: 
case 6: ae ta 
“DrOpram status — MAA, |: 
break: 


Caos ie bar(-200.0.200.0,4slidebar.&barvalue,trans rate- ay 
if (shdebar == IN) 
{ 


ordered rate->>x = barvalue; 


} 
break: 


case & bar(-100.0,100.0,&slidebar.&barvalue,trans rate->y): 
if (shdebar ~— IN) 
{ 


ordered rate->v = barvalue; 


break. 


case 9: bar(-1.0,1.0.&shidebar,&barvalue,rot rate->z); 
if (shdebare==——1s\)) 
{ 


ordered rate->z = barvalue: 


} 
break; 


case 10:  °* Future expansion * 


break: 


case 1]: /* Future expansion * / 
break; 


case 12: ‘* Future expansion */ 


break; 


default: = color( BLACK): 
} 


popview port (); 
popmatrix(): 


\ -* end of driver command *, 


i114 


driver.c 


we gh &h Sp Sos Gp ERR SED GaSe RS EP oe Ge SSeS eR Say BR ES ek) PERM E) Gy SS Ee RS he oe ee RA ESE) BB ES 53 Ge es eee Rp eh So oe ee RR TES be ede ae tek ge ak oe ae Sh ee ee oe ee ke ed 


bar(minval, maxval.slidebar,barvalue.actualvalue}) 
float minval, maxval.*barvalue,actualvalue: 
int “slidebar: 
: 3 
REPISLEr 1; 
char str 20); 
int x 
Static int barlevel; 
/* Draw the sliding bar. 
cursoff(): 
color(BLACKk); 
rect fi(9,69,90,690): 
color(RED); 
recti(10,70.30,670); 
for (1=0;1<5:1+—}) 
{ 
move2i(30,70 + 1*150): 
draw21(40,70 + 1° 150}; 
cmov21(34,.73 + 1°150)}: 


"%6.1f",minval + 1*(maxval-minval) / 4.0): 


* 


sprintf(str, 
charstr(str); 


} 


curson(); 


/* Check the location of the cursor. If it is inside the 
sliding bar, then calculate the value for its position. 
x = getvaluator(MOUSEX): 
y = getvaluator(MOUSEY),; 
Melo <x && x < 30 kk 70< y && y < 670) 
{ 
barlevel = y: 
*slidebar = IN: 
*barvalue = minval + (maxval - minval)*(y - 70)/600.0; 


4 / 


else 
{ 

*shidebar = OUT; 
} 


’* Draw the bar showing the actual level. */ 


color(RED); 
rectf(15.0,70.0,25.0,(370.0+ 600.0* actualvalue/(maxval-minval))); 


/* Draw the bar showing the ordered value. * / 


color( YELLOW); 
recti(11,70,29,barlevel); 
mae end of bar *, 
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joystick(trans rate.rot rate.ordered vel mag.ordered vel dir,buttonflag) 


vector ‘trans rate. ~*~ translation rates of the center of gravity in body coordinates * / 


x! 


*rot rate: ~*~ body Euler angle rotation rates 


float ‘ordered vel mag, ;* ordered velocity of cockpit (magnitude) * 


“ordered vel dir: ,* ordered velocity of cockpit (direction) */ 


int *buttonflag: ,* indicator for middle mouse button */ 
Int Xayi: 
float vx.vy. ,* velocity of cockpit in body coordinates */ 


magn.dir; * magnitude and direction of cockpit velocity vector * 


* 


Display the steering box. 
eolor{ BLUE): 
recti( 100.80,400,380): 
Display the grid */ 
for (= iodo) 


\ 
move2(90.0+1*20.0,80.0): 
draw 2(90.0—1* 20.0.380.0); 
for Glico) 
{ 
move2{100.0,80.0+1* 20.0); 
draw 2(400.0,80.0~1* 20.0); 


Display instructions. 
cmov2i(110.65): 
charstr("Hold the middle button down"); 
cmov2i(110,50): 
charstr("to control the joystick"); 
* Display the current velocity of the cockpit. */ 
vx = trans rate->x; 
vy = rot rate->z * HALFLENGTH + trans rate->y: 


magn = sqrt(vx*vx + vy*vy); 


dir = atan2(vy,vx); 
if (vx @= 0.0) 
\ 

dirgm,020: 
} 
linewidth(5): 
colér| y EEEONS ). 


move2(250.0,80.0); 
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if (vx =—0.0) 
{ 

dirv= 0.0: 
j 
linewidth(5): 


color( YELLOW), 
move2(250.0,&0.0); 
draw 2((250.0-400.0* dir) ,(80.0+ magn *3.0)); 


-* Check the location of the cursor. */ 
x = getvaluator(MOUSEX); 
y = getvaluator(MOUSEY); 
fae buttoniag —— DOWN) 
{ 
if (100< x && x < 400k&& 80< y && y < 380) 
{ 
“ordered vel mag — (y-80) 3.0; 
“ordered vel dir = (250-x), 400.0, 


\ 
j 


Display the ordered velocity of the cockpit. */ 
linewidth(3): 
color(RED): 
move2(250.0,80.0); 
draw2((250.0 - 400.0 * “ordered vel dir),(&80.0 + *ordered vel mag * 3.0)); 
pes idehi ): : — 
* end of joystick * 
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This is a function for the iris2400 program walk.c. 
steering.c 


Relle Lyman 26 Mar 1987 
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#include "gl.h" 
#include "device.h" 
#include "walk.h" 
#include <stdio.h> 
#include <« math.h - 


steering conv(ordered rate,ordered vel mag.ordered vel dir,azimuth,tx,ty.fh) 


This function calculates converts ordered head velocity to 


ordered body translation and rotation rates. *. 


float “ordered vel mag. * ordered velocity of the cockpit (magnitude) *, 


“ordered vel dir. * ordered velocity of the cockpit (direction) */ 
“azimuth. * body azimuth angle (radians) * 
oN; OS: ‘ current position of the body's center of 


gravity (in earth coordinates} *- 


vector “ordered rate. * ordered forward and lateral translation 
rates and azimuth angle rate * / 

thea * selected foothold (in earth coordinates) * / 

\ 

float hx.hy, * current head (cockpit) position (earth coord.) */ 

dhx,dhy, * desired head position (earth coord.) */ 
fheen x,fhcen y.  * foothold centroid (earth coord.) */ 
degx.degy, ‘* desired center of gravity (earth coord.) */ 
dazimuth, * desired azimuth angle (earth coord.) *, 
diffazm:; /* difference between desired and current azimuth */ 


vector desired rate; /* desired earth translation rates and azimuth 
angle rate * 
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steering conv 


‘* Note: This module uses a level terrain assumption. * 
‘* Calculate current head position (earth coordinates). * 
Pee tx  HALPUE NGO” cos( *azimuth); 
hy = *ty + HALFLENGTH * sin( *azimuth): 


‘* Calculate the desired head position (earth coord.). * / 
dhx = hx + DELTA TIME * ‘ordered vel mag * cos{ *ordered vel dir + *azimuth): 
dhy = hy + DELTA TIME * “ordered vel mag ~* sin({ *ordered vel dir + *azimuth); 
'* Calculate the foothold centroid. (Forward gaits only) */ 
fheen x = (fh.3|.x+fh 4].x+fh'5).x+fh{6].x) /4.0; 
fheen_y = (fh.3i.y+fh 4].y+fhj5].y+fh/6].y) /4.0; 
‘* Calculate the desired azimuth angle. */ 
dazimuth = atan2((dhy-fhcen y),(dhx-fhcen x)): 
diffazm = dazimuth - *azimuth: 
‘* Adjust the difference to a value between pi and -pi. 
if (diffazm < -3.14159} 
\ 

diffazm += 6.2831853: 
} 
if (diffazm > 3.14159) 
{ 

diffazm -= 6.2831853: 


-* Calculate the desired center of gravity. */ 
degx = dhx - HALFLENGTH * cos(dazimuth); 
degy = dhy - HALFLENGTH ®* sin(dazimuth); 


t 


desired rate.x = (dcegx - *tx)/; DELTA TIME; 
desired rate.y = (degy - *ty)/ DELTA TIME, 
desired rate.z = diffazm/DELTA TIME; 


,* Calculate the desired rates (earth coord.). */ 


/* Convert to body translation and rotation rates. *, 


ordered rate->x - cos{ “azimuth) * desired rate.x 
~ sin{ “azimuth) * desired rate.y; 
ordered rate->y = cos( *azimuth) * desired rate.y 
- sin( *azimuth) * desired rate.x; 
ordered rate->z = desired rate.z: 7 


$  * end of steering conv */ 
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This is a routine for the irts-2400 program walk.c. 
status.c 

This routine creates a status report to be displayed 

on the viewing screen beneath the ASV. 

Relle Lyman 27 Mar 1987 
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#include "gl.h" 
#include "device.h" 
#include "walk.h" 


status report(ordered rate,trans rate.rot rate,b footpos,period.alpha,gamma. 
theta.slow flag, roll.elev ‘azimuth, txstyt2) 


int *slow flag; ‘* flag indicating deceleration is needed *. 
Angle theta7. * leg component angles * 

alpha 7. 

gamma é@ ; 
float *period, -* period of leg cycle * 

Rat HZ, _* position of body in earth coordinates * 


“roll.*elev,*azimuth; /* body Euler angles * 
4 g 


vector “rot rate.  ,* body rotation rates * 
‘trans rate, /* body translation rates * 
* 


*ordered rate. .* ordered lateral and longitudinal and yaw rates 
b footpos 7: §* foot position in body coordinates * 


int 1k: 


char str fpx 7, 100 .str fpy|7)|100),str_fpz. 
str orx 100 .str ory|100 ,str orz!100), 
Str Urx' 100 ,str “ury|100) str _t17| 100 , 
str rrx/100|,str_rry!100|,str_rrz|100}, 
str alpha 7 100) str _gamma 7 |100),str_ theta/7 100), 
str _period. 100], str slow |100., 
str tx! 100),str ty) 100), str t2/ 100), 
str roll/100/,str_azimuth{100 ,str_elev|100:: 


7|(100], 


4 
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sprintf(str orx."°(7.2f"' ordered rate->x); 

sprintf(str ory.''®¢7.2f" ordered “rate->y): 

sprintf(str orz."%7.2f—" Orica Tate >z): 
2 


zi 
% 
sprintf(str trx. Zi thats _rate-> x) 
sprine@i(str try. f trans rate->y); 
sprintf(str trz. .2f" trans rate->z); 
Zap rot. rate->x): 
2f",rot_rate->y); 


sprintf(str_rrx,' 
sprintf(str rry.' 
sprintf(str rrz."%7.2f".rot rate->z): 
sprintf(str period, "9.58" .* period): 
rimti(str tx, een. 2f", tx); 

Spi (str ty, er. 2f",*ty); 

sprintf(str tz, "C7 fei CZ) 

sprintf(str roll, "¢7d" rate aero Fo as-O)))e 
eeumti(str azimuth. 2eid",((int) (*azimuth * 573.0))); 
sprintf(str elev."°o7d",((int) (*elev * 573.0))): 


bet 
we 
oe 
ns 
% 


AT. 
ite. 
fe 
7 
= 
ie 
a. 


for (k=1;k<7;k++) 

{ 
sprintf(str fpx k ."%7.2f",b footpos k’.x); 
sprintf(str fpy k ."%7.2f" b footpos|k!.y); 
sprintf{(str_fpz|k',"%7.2f".b footpos{k!.z): 
sprintf(str_ alphalk "7d" alphaik ); 
sprintf(str gammak , "ead" gamma k ): 
sprintf(str theta k!."%7d" theta k)); 


} 


pushmatrix(); 
view port(0.400,0,767); 
ortho2(0.0,400.0,0.0,767.0): 
color(BLACK): 
rectfi(10.10,400.370); 
color(YELLOW); 
rect fi(20.20,390,360); 
color(BLACK); 
cmov 21(220,340): 
ehianstr( XX"); 
emov2i(280.340): 
etarstr(" ¥"); 
emov 2i(340,340): 
charstr("Z"): 


emov 21( 30,025): 
charstr("ordered rate"); 
emov 21(30,310); 7 
charstr("trans rate"); 
cmov2i(30,295); 
charstr("rot rate"); 
cmov2i(30,280); 
charstr("position"): 
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cmov21(2 10,265): 
charstr( ROEL), 
cmov 21(260,265); 
charser("ELEY 2"): 
cmov 21(310,265): 


charstr("AZIMUTH"): 


cmov21(30.250); 
charstr("current attitude"); 
CMIOV 21100200) 
charstr("'ordered attitude"): 


cmov 2i(30,210); 


charstr("period"): 


if(*slow flag == SLOW) #* moving too fast” 
emoy 21( 750.220); 
color(RED): 
charstr( TOO FAST"). 
color(BLACh), 
} 


cmov 21(30,185): 

charstr("x ft pos (1-3)"): 
cmov21(110.170): 
charstr("(4-6)"); 

Emo. 2ilol. loo), 

charstr("'y ft pos (1-3)"); 
cmov 21(110,140); 
charstr(''(4-6)"), 

cmov 21(30,125); 

charstr("z ft pos (1-3)"); 
cmov21(110,110),; 
charstr("(4-6)"'); 

cmov 21(30,95): 
charstr("ALPHA = (1-3)"); 
cmov2i(110.80}); 
charstr(''(4-6)"); 
emov21(30.G.)), 
charstr("GAMMA = (1-3)"): 
emov2i( 11050), 
charstr('(4-6)"); 

emoev 21(g0.35)). 

charstr| pie A aC 3-3). 
emovwz(11G92G)) 
charstr(''(4-6)"): 


status.c ~ 


emov 21(180.325}): 
charstr(str orx); 
emov2i(240,325); 
charstr(str ory); 
cmov2i( 300,325): 


charstr(str orz): 


cmov2i( 180,310); 
charstr(str trx); 
cmov2i(240.310): 
charstr(str_ try): 
emov 2i(300,810): 
charstr(str_ trz); 


cmov 21i( 180,295): 
charstr(str rrx): 
emov2i(240,295): 
charstr(str rry): 
emo 21( 300,295): 
charstr(str_rrz): 


cmov2i(180,280): 
charstr(str_ tx); 
cmov 21(240,280): 
charstr(str ty); 
emov2i(800,280); 
charstr(str tz); 


cmov2i( 180,210): 
charstr(str_ period); 


emov2i( 180,250); 
charstr(str_ roll); 
cemov2i( 240,250); 
charstr(str elev): 
emov2i(300.250): 
charstr(str azimuth): 


status.c 


for (i= lysis 4ii—+) 

{ 
al Oe 0: 
STM. 2 itera: 
charstr(str fpx.1); 
cmov2i(k.170); 
charstr(str fpxi+3}); 
cmov2i(k,155); 
charstr(str fpy'i,): 
cmov2i(k, 140); 
charstr(str fpy 1+3)); 
cmov2i(k, 125); 
charstr(str fpzi): 
EMovci(k eho): 
charstr(str fpzi+3)); 
cmov2i(k.95): 
charstr(str alpha}: 
cmov2i(k,&0); 
charstr(str alpha i+s ): 
cmov2i(k.65); 
charstr(str gamma 1}; 
emov2i(k.50): 
charstr(str gammaji-—3 ); 
emov2i(k,oo); 
charstr(str theta ij); 
cmov21(k.20); 
charstr(str theta i+3 ); 


} 


popmatrix(); 
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This is a function for the iris 2400 program walk.c. 
support.c 


Relle Lyman 21 Aug 1986 
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pinclude "el.h" 
+#include "device.h" 
#include "walk.h" 
#include <stdio.h> 
#include <math.h> 


support plane(spe} 


‘* This module will compute a new estimated support plane based on 
the position of the supporting legs. As a temporary measure it 
is assumed the support plane is flat and at "sea level" (i.e. 
= 0). The equation for the plane is Ax~By+Cz+D=0. */ 


f 


plane “spe; “ estimated support plane in earth coordinates * 


spe->a = 0.0: 
spe->b = 0.0; 
spe->c = 1.0; 
spe->d = 0.0; 
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This is a function for the iris 2400 program walk.c. 


body rates.c 


Relle Lyman 19 Apr 1987 
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#include "gl.h" 
+include "device.h" 
#include "walk.h" 
zinclude <stdio.h> 
zinclude <math.h > 


body rates({rot rate.trans rate.spe.h,invh,ordered rate.tx,ty,tz, 
roll.elev,azimuth) 


4 


vector “rot Tate. 


plane 


float 


* 


* 


int 


float 


plane 


This function computes the body rotation and translation rates. * 


* 


fetationanate 
*trans rate. “translation wate 
*ordered rate: .* ordered x translation, y translation, 


and z rotation rates 
* spe; ‘* support plane in earth coord * / 


h.4 4], /* homogeneous transformation matrix * 
invhi4; 4), ‘* inverse transformation matrix * 

tx, *ty,*tz, |* position of body in earth coordinates * 
roll.*elev.*azimuth; * body Euler angles * 


Ie 
eta, “ body plane attitude wrt earth plane * 
eta d, * desired body plane attitude *' 


height, * distance form CG to est. support plane */ 
height d, * desired height *, 


gamma. “ angle between desired and present body unit normal vectors * 
kx, -* x component of rotation unit vector in body coord *, 
kv, /* y component of rotation unit vector in body coord */ 
ka, »* control law gain *- 
a,b,c, /* body plane desired unit normal in body coord *, 
length, /* rotation vector normalizing factor * / 
$ 


celev,selev telev, /* sine.cosine,tangent of elev * 


* 


croll.sroll,cazim,sazim, /* sin and cos of roll and azimuth * 


mM, 


spb: “support plane in body coordinates * 


vector db unit norm. /* desired body plane unit normal in earth coordinates */ 


trans rate e, '* Translation rates in earth coordinates * 
rot rate.e; ‘* Rotation in body Euler rates */ 


126 


* 


body rates.c 


* Calculate the body plane attitude. (level ground assumption!)* 
Atay = 000: 


-* Calculate desired body plane attitude {level ground assumption!)” 
eta d = eta: 


-* Calculate the desired body clearance. (level ground assumption!)* | 


height d = HO: 


* Calculate the support plane coefficients in body coordinates. *, 
Be ispb| T = |spagl ~h : 
spb.a = spe->a * h/0]{0| + spe->b * hi1)/0, + spe->c * hi2 [0 + spe->d * h/3/(0); 
spb.b = spe->a * h[0]/1] ~ spe->b * hj1|[1, + spe->c * h'2!1: + spe->d * hi3 /1); 
spb.c = spe->a * hj0|/2' ~ spe->b * hj1/!2| ~ spe->c * h/2/'2. + spe->d * hj3'/2; 
spb.d = spe->a * h[0,\3) + spe->b * hj1|[3' + spe->c *h 2/3 + spe->d * h 3 (3); 
‘* Height of body CG above support plane *, 
height = spb.d: 
* Calculate desired unit normal for the body plane in earth coord. */ 
m — sqrt(spe-->a * spe->a + spe->b * spe->b)}; 
‘™ check for division by zero */ 
if (m>0.0) 
\ 

db unit norm.x — spe->a ~ sin(eta d),/ m; 

db unit norm.y = spe->b * sin(eta d) / m; 


else 


{ 


db unit  norm.x = 0.0; 
db unit norm.y = 0.0; 


} 


db umit norm.z = cos(eta d); 
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body rates.c * 
* Transform the desired unit normal vector of the body plane 
from earth to body coordinates. a,b,c T=invhr*b unit norm * 
Note: invhr is the inverse of the rotational transformation 
submatrix (first three rows and columns of h). *. 
a = invh 0 '0*db unit norm.x + invh 0) 1 *db unit norm.y + 
invhiO 2!*db unit norm.z, 
b =invh.},|O0 ‘db unit norm.x + invhi}|!1 *db unit norm.y + 
mvh 1 °2)*db unit norm.z: - 
¢ = invh 210 *db unit norm.x + invh 2/'1/*db unit norm.y — 
inva 22" db unit norm: 


* Control law yielding an exponential response * 
* dgamma/dt = -ka * gamma a 
ka = 1/TIME CONSTANT 1; 

gamma = acos(c): 

length = sqrt(a*a + b*b); 

if (length < .OOOOL) 


KaeenOe 
Ky cs: 
else 


components of rotation unit vector in body coordinates * 
kx = -b length: 
ky = a‘length: 


* 


Calculate rotation and translation rates * | 

trans rate->z = -ka * (height d - height): 

rot rate->x = -ka * kx * gamma: 

rot “rate->y = -ka ~ ky ~ pamma: 
* Rate = dt * acceleration ~ old rate */ 

trans rate->x = DELTA TIME * (ordered rate->x - trans rate->x) 

7 TIME CONSTANT 2 + trans rate->x; 
trans rate->y = DELTA _TIME * (ordered rate->y - trans rate->y)/ 
TIME CONSTANT 3 + trans rate->y: 
rot rate- -z = DELTA TIME * (ordered rate->z - rot_rate->z) ’ 
| TIME CONSTANT 3- rot rate->z; 

* Conversion to Earth coordinate translation rates. * / 

croll = eos| nol: 

sroil) sini roll); 

telev = tan(“‘elev); 

celev = cos(“*elev); 

seley = sin(*elev); 

CazIM = COst azimuin): 

sazim = sin(*azimuth): 
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* 


body rates.c * 


trans rate e.X = trans rate->x * croll*cazim + 
trans rate->y * (cazim*sroll*selev - sazim*celev) + 
trans rate->z * (sazim*selev ~— cazim*sroll*celev):; 


* croll*sazim + 


trans fate @.v = trans rate->x 
trans rate->y * (sazim*sroll*selev ~ cazim*celev) + 


trans rate->z * (cazim*selev — sazim*sroll*celev); 


trans rate e.z = -trans rate->x ™* sroll + 
trans rate->y * croll*selev — 
trans=rate->z * cazim celev: 


‘* Conversion to body Euler rates * 
rot rate e.X = rot rate->x + rot rate-=y * telev * sroll + 
rot rate->z ~ telev * croll: 


* 


rot rate e.y = rot rate->y * croll- rot rate->z * sroll; 


* 


rot rate e.z = rot rate->y “ sroll / celev ~ 


rot rate->z * croll / celev: 


‘™ Integration routine *, 

ft += trans rate e.x ~ DELTA TIME: 
“t} += trans rate e.y ~ DELTA TIME: 
(tz +— trans rate e.z2 ~ DELTA TIME; 
“roll += rot rate e.x ~ DELTA TIME; 
Bclev +—rot rate ey DELTA-TIME; 
“azimuth += rot rate e.z * DELTA TIME: 


/* Update the H matrix */ 


Croll = cos( roll): 

ero = sin{ roll): 

telev = tan(*elev); 
celev = cos(*elev): 
selev = sin(*elev); 
eazi1n) — cos( azimuth); 
sazim = sin(*azimuth): 
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. body 


h0)|0, 
hol 
h 0/2 
h Oj|3) 
h 1/{0) 
h 11 
hj 1}/2/ 


hi2il0 
hi 2) {11 
hy2 | 2: 


hi2u3, 
70.0; 
sO): 
= 0.0: 
= 1,0: 


h 3| 
h3 
h 3 
h 3: 


-| 


oe a 





i) 


rates.c ~ 


- croll*cazim: 

— cazim“‘sroll]*selev - sazim'‘celev. 
— sazim’selev + cazim‘sroll*celev: 
ad tx: 

= sazim"croll: 

: Ss s = * 

= cazim*celev + sazim’sroll*selev: 
= sazim’sroll*celev — cazim*selev; 


h!1i)3! = 


atv 


= <Sroll: 
= croll*selev: 


*croll ¢eley: 
Uz: 


* inverse homogeneous transform matrix * 
for (i=0; 1< 3; i-—) 


for (J 


=0; j<°3; j--+) 


invh ijj) = h jis 


iInvivselte = 0.0: 

invh|i/3) = -(hOlla *h'O}/3) + heeflie*hj iis) + hi2]|i*h[2h30); 
} 
Ged | in = 10: 


“end of body rates * 
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This is a function for the iris 2400 program walk.c. 
con work vol.c 


Relle Lyman 19 Apr 1987 
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include "gl.h" 
#include "device.h" 
#include "walk.h" 
sinclude <stdio.h> 
sinclude <math.h> 


con work vol(cwv.b footpos.leg status,warning) 


‘* This module will compute a new constrained working volume for 
improved stability on rough terrain. Currently all values are 
set for a perfectly flat support plane. Dimensions are 


expressed incm. */ 


work vol cwv|7; /* constrained working volume in body coordinates * 


vector b footpos7; /* foot position in body coordinates * 


int leg status 7./* status of leg (supporting, liftoff, transfer. 
placement) */ 
*warning; ” flag indicating supporting leg 1s outside of 


the working volume * / 


int 1: 


arming = OFF: 
for (i=1:1<7;1+4+) /* check each leg */ 


{ 
if (leg status i. == SUPPORTING) 


{ 
if ((b footpos 1.x 
(b footpos!i!.x > cwv|i].x.max)| 
(b footpos|i.y < cwv|i].y.min)}| 
(b footpos'i .y > cwvil.y.max)|, 
(b footpos|i.z < cwv/i).z.min)| 
(b footpos1.z > cwv|1..2.max)} /* outside limits 


Swarming = ON; 
} 


} 


< cwv(i].x.min)}| 


A 


A caer 


* 
é 
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con work vol.c *, 


_* 


if (‘warning == ON) - = 
{ 

pushmatrix{): 

pushviewport(}); 


viewport(0,130,0,80); 
ortho2(0.0,130.0.0.0.80.0); 
color(RED); 

rect fi(10,10.130.70); 
color(BLACK): 
emov2i(10.30); 

charstr(’ ON 1s DEC yer"): 
popviewport(); 

popmatrix(); 
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This is a function for the iris2400 program walk.c. 
opt period.c 


Relle Lyman 29 Apr 1987 
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#include "gl.h" 
zinclude "device.h" 
#include "walk.h" 
#include <stdio.h> 
#include < math.h> 


optimal period(legphase,b footpos,rot rate,trans rate,cwv,leg status,period} 


‘* This function computes the optimal period for walking. */ 
vector “rot rate. ‘* body rotation rate * | 

*trans rate. ‘* body translation rate *, 

b footposi7 ; °* position of foot in body coordinates * 


work vol cwv 7: /* constrained working volume ” 


float legphase 7. /* phase of leg * 
* period: /* body support period * / 


int leg status|7]; /* status of leg 0 = supporting */ 


vector b footvel: /* foot velocity * | 
Goat fx.fy.fz. i reo position ~, 
tmin, /* minimum temporal kinetic margin *, 
Px ty tz, /* temporal kinetic margins * / 
an /* distance to cwv limit * 
speed, /* magnitude of body velocity ~* / 
fs period, /* forward support period * / 


bs period, /* backward support period */ 
min fs period, /*~ minimum forward support period */ 
min bs period, /* minimum backward support period */ 
* / 


fs phase, /* forward support phase 
Z5 


bs phase. /* backward support phase 
mvxX,mvy,mvz; 


iit (1. 
minleg: 


static int gait=>FORWARD,; °* Wave gait direction *, 


/ 
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* 


optimal period 
* Initialize variables * 

tmin = LONG ME; 

min fs period = LONG TIME: 
min bs period = LONG TIME: 
“ For each leg compute the maximum instantaneous support period. * 
for (i lemiee le) 


\ 
Support check */ 
if (leg status;i) == SUPPORTING) 
\ 


Lesh x 


Compute foot velocity. 

b footvel.x = -(trans rate->x)+(rot rate->z * b footposij.y) 
- -(rot rate->y * b footpos i .z): 

b footvel.y = -(trans rate- >y)-(rot rate->z * b footpos i .x) 
- +(rot rate->x * b foot pos! i!.z): 

b footvel.z = -(trans rate-~-z)+(rot rate->v * b foot pos 1.x) 
7 -(rot rate->x * b footpos 1 .y); 
* Check to see if foot is in cwv. * 

ix = b foot posmmx: 

fy = b foot pos iy; 

fz = b footpos 1.2: 

if ((fx< cwv 1.x.min) : (FR cwvi .x.maxr} 

(fy< cwvii.y.min); (fy cwv.1|.y.max) 


* 


(fz< cwv il.z.min)| (fz > cwv{il.z.max)) outside cwv */ 


Cine a le 


else 


‘* Compute distance to x limit and the temporal 
kinetic margin in the x direction. */ 

if (b footvel.x > 0.0) 
d cwv i|.X.max - fx: 
tx =d_/ b footvel.x; 


else if (b footvel.x < 0.0) 


olen ee i].x.min; 
ix = cael -b footvel.x: 


else 


{ 
tx = LONG TIME; 
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* optimal period * 


=x 


/* Check for minimum value. *, 
if (tx<tmin) 
tmin = tx; 


} 


/* Compute distance to y limit and the temporal 
kinetic margin in the y direction. */ 
if (b footvel.y > 0.0) 


{ 
= cwviil.y.max - fy; 
ty =d/ b footvel.y; 
} 
else if (b footvel.y < 0.0) 
| 7 
d = fy - cwviij.y.min; 
ty = d / -b footvel.y; 
} 


else 


ty = LONG TIME; 


/* Check for minimum value. */ 
if (ty<tmin) 


{ 


min —< ty; 


‘* Compute distance to z limit and the temporal 
kinetic margin in the z direction. */ 
if (b footvel.z > 0.0) 


{ 


d = cwviil.z.max - fz; 
tz =d / b footvel.z; 


else if (b footvel.z < 0.0) 


d = fz- cwv|il.z.min; 
tz =d / -b footvel.z; 


} 


else 


tz = LONG TIME; 


/* Check for minimum value. */ 
if (tz<tmin) 


Cine — cz. 
} 


} /* end inside cwv */ 
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x 


optimal period 
“ Compute the support phase for forward and backward gaits. * 
fs phase = legphasei BETA: 

bs phase - (BETA - legphase;i) BETA; 


* Compute support period. */ 
fs period = (tmin-0.1)/(1.0 - fs_phase); 
bs period = (tmin-0.1)/(1.0 - bs phase); 
“Find the minimum support period. * | 
if (fs period < min fs_ period) 
{ 

min fs period = fs_ period; 


j 


if (bs period < min bs_ period) 
min bs period = bs period, 
\ -* end support check * 
\ —* end leg loop * 
‘* Choose gait. * 
speed — sqrt(trans rate->x * trans rate->x + 
trans rate->y * trans rate->y); 


if ((speed OUTER LIMIT)&&(trans rate->x > INNER LIMIT)) 


* 


gait - FORWARD. 
else if ((speed< OUTER LIMIT)&&(trans rate->x< -INNER LIMIT)) 
{ 
gait = BACKWARD, 
} 
else 
{ 
/* No gait change. */ 


} 


if (gait == FORWARD) 
{ 
“period = min fs_period; 


} 


else 


\ 


“period = min bs _ period; 


} 


*x 


end optimal period */ 
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This is a function for the iris 2400 program walk.c. 
decelerate.c 


Relle Lyman O4 May 1987 
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#include "gl.h" 
#include "device. h" 
#include "walk.h" 
#include <stdio.h> 
#include <math.h> 


decelerate(rot rate,trans rate,period,slow flag,min_ period) 


* 
/ 


This function computes the foot transfer rate and slows the 
vehicle if the maximum rate is exceeded. */ 


xi 


vector *rot rate, /* body rotation rate 
*trans rate: ,* body translation rate */ 


/ 


/* optimal period for the leg cycle *; 


float *period. 
*min period: ‘/* minimum leg period */ 


int “slow flag: ‘* flag indicating vehicle has been slowed. */ 


int a 


float transfer time; /* time from liftoff to placement * / 


if(* period < *min period) /* slow down */ 
{ 
*slow flag = SLOW; 


*period = *min period; 


trans rate->x *= .95: 
trans rate->y *=.7: 
trans rate->z *= .95; 
rot rate->x *= .95; 
rot rate->y *= .95; 


hot rate->2% *= .7- 
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* decelerate.c * 


* display warning on screen ~ 
pushmatrix(): 
pushview port():; 
view port(200,330.0.80); 
ortho2(200.0,330.0,0.0.80.0); 
color[ Y ELLO We}; 
rectfi(210,10,330,70): 
color(BLACh); 
cmov2i(210,30); 
écharstr(  DECELERATION |; 
popviewport(); 
popmatrix(); 

} 

else 
*slow flag = NORMAL: 


j 


\ * end of decelerate” 
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This is a function for the iris2400 program walk.c. 
leg phase.c 


Relle Lyman 24 Aug 1986 
Ne Er Se ae RA ee x 
#include "gl.h" 

#include "device. h" 
#include "walk.h" 

#include <stdio.h> 
#include <math.h> 


leg phase( legphase, rel legphase, period) 


{* * 


This function computes the phase for each leg. */ 
float legphase'7., /* phase of leg */ 
rel legphase 7. /* relative phase of leg */ 
* period: /* body support period */ 


static float bodyphase = 0.0; /* kinematic phase of body * 


/ 
f 


float modulus one(); modulus one function */ 


int i: 


/* Calculate new body phase. * 


bodyphase = modulus one(bodyphase + DELTA TIME/(* period) ): 


/* Calculate new phase for each leg. (mod1) */ 
fom (i= 1; 1< 7: 1+) 


legphase|i = modulus one(bodyphase - rel _legphase/i)); 


} /* end of leg phase */ 
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This is a function for the iris2400 program walk.c. 
ft traj.c 


Relle Lyman 19 Apr 1987 
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#include "gl.h" 
zinclude "device.h" 
#include "walk.h" 
#include <stdio.h> 
¢include <math.h> 


foot trajectory(legphase.period,leg status.footpos.b footpos.fh, 
oldfh,invh,h,cwv,trans rate,rot rate,selected gait) 


This function calculates the trajectory for each foot. */ 
float legphase'7..  * Phase of individual leg */ 
“period, * Optimal period sa 
hid! 4, ‘* Homogeneous transformation matrix ~*/ 
invhidi'4 : * Inverse transformation matrix  _*/ 
vector footpos'7 . 
b footpos 7, 
fh7. * Foothold selection (earth coordinates) */ 
oldfh 7, * Old foothold selection (earth coordinates) * 
“trans rate, ©* Body translation rate */ 
*rot rate: * Body rotation rate *, 
WORK Vol cCwvod 
int leg status:7.. °* State of individual leg *. 
“selected gait; _* Desired tripod gait */ 
float trans time, »* Time required to go from leg liftoff to leg touchdown */ 


end lift phase, /* Point in transfer phase where liftoff ends */ 
begin place phase, * Point in transfer phase where placement begins */ 
trans phase, /* Leg transfer phase */ 


static int liftoff flag 7/=OFF. ,* Indicates first time entering the 
subroutine in the current leg cycle. */ 
transfer flag 7 —OFF, 
place flag 7 =OFF, 


ix 


static vector d footpos 7); Desired foot position * 


int ei _* Leg number *, 


140 


foot trajectory * 
* Calculate the time required to move a leg from liftoff to 
touchdown. (Transfer time) */ 


trans time = (1.0- BETA) * ABS(*period); 


/* Calculate phase points marking change of transfer mode 
(direction). Note: Modify later to account for transfer 
time. *; 

end lift phase = 0.2; 

begin place phase = 0.8; 

‘* For each leg */ 

for (i=1; 1<7; i++} 


{ 


/* Calculate transfer phase. * 
emit 0.0 place — 1.0 77 
if (*period < 0) 
{ 
trans phase = {1.0- legphasei)/(1.0- BETA); 
} 
else 
{ 
trans phase = ( legphase|i - BETA)/(1.0 - BETA); 


} 


= 


Find the leg transfer state. * 
if (trans phase < 0.0) 
{ 

leq status» — ot PROR LING. 

support trajectory(liftoff flag.place flag,transfer flag footpos, 

b footpos.invh.i);. 

j 
else if (trans phase < end lift phase) 
{ 

leg status(i] = LIFTOFF; 

lift trajectory(liftoff flag,place flag,transfer flag,footpos, 

b footpos,invh,&trans time,&trans phase,&end lift phase,1); 

} 


else if (trans phase < begin place phase) 
{ 
leg statusij) = TRANSFER FORWARD, 
transfer trajectory(liftoff flag,place flag,transfer flag,footpos, 
b footpos,h,invh.&trans time,&trans phase, 
& begin place phase,i,cwv,trans rate,rot rate,fh.oldfh. 
period,selected gait); 
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foot trajectory * 


else “end place phase < trans phase < 1.0 * 
\ 
leg status 1 = PLAGEMIENT, 
placement trajectory(liftoff flag.place flag.transfer flag. foot pos, 
b footpos,invh,&trans time,&trans phase.i); 
} 
} 


} /* end of foot trajectory */ 


| 


+ 


foot trajectory 
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lift trajectory(liftoff flag,place flag.transfer flag.footpos,b footpos. 
invh,trans time,trans phase.end lift phase.leg number) 

* This function calculates the trajectory of the foot while it is 

being lifted from the ground. It is called from foot trajectory()}.* / 


/* Present foot position in earth coordinates * / 


* / 
/ 
/ 


vector  footpos 7], 


b footpos|7]; /* 


Present foot position in body coordinates 


float “trans time, 
“end lift phase, 
“trans phase, 
invh'4]/4:; /* Inverse homogeneous transformation matrix * / 


int liftoff flag,7 , /* Indicates the first time entering subroutine 
in the current leg cycle. */ 
transfer flag 7 , 
place flag 7), 
leg number; 


float lift time: 
int i 


static vector d footpos 7: * Desired foot position 
in earth coordinates * 


1 = leg number: 


* Calculate the desired footposition. * 


if ( liftoff flag i) != ON) 
{ 
d footposii).2 = footpos|i].2 + FOOTLIFTHEIGHT; 
liftoff flagli) = ON; 
transfer flag|i] = OFF; 
place flagii] = OFF; 


‘* Calculate the time required to reach the desired height 


from the present foot position. *, 


hft time = “trans time * (“end lift phase - *trans phase); 
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* foot trajectory * 


-* Calculate the new foot position. (Earth Coordinates) * 


if (DELTA TIME < lift time) 


footpos|i .z ~= (d footposii .z - footpos i|.z) 


* DELTA TIME / lift time; 
j 


else '* Last increment of time 


{ 


footpos:i).z2 = d footpos i).z; 


} 


= Fl 


_* Transform to body coodinates. *, 
.* \b footpos i)|T = invh * [footposli)|/T */ 
transform point(b footpos.invh footpos.1); 


x / 


} | * end of lift trajectory 
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* * 


foot trajectory 
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transfer trajectory(liftoff flag.place flag.transfer flag.footpos. 
b footpos.h,invh. trans time.trans phase.begin place phase, 
leg number.cwv,trans rate,rot rate,fh.oldfh,period,selected gait) 


/* This function calculates the trajectory of the foot during the 
phase in which the foot is transfered forward. The function 
is called from foot trajectory ().*/ 


vector footpos 7), /* Present foot position in earth coordinates */ 
* / 


~~ 


b footpos|7,, ,* Present foot position in body coordinates 


fli) 7}, /* Selected footholds (earth coordinates) */ 
oldfh 7,  /* Old selected footholds (earth coordinates) */ 
*trans rate, ;* Body translation rate */ 
‘rot rate; /* Body rotation rate */ 
work vol cwv\| ii: * Constrained working volume in body coordinates * / 
float “trans time, 
“begin place phase, 
“trans phase. 
* period, /* Optimum period of gait */ 
h4 4, /* Homogeneous transformation matrix */ 
invh 4, 4]; _, * Inverse transformation matrix */ 
int liftoff flag 7, /* Indicates the first time entering subroutine 


in current leg cycle */ 
transfer flag 7, 

place flag 7. 

“selected gait, ©* Desired tripod gait * ' 
[eee nium ber 


float trans fwd time, /* Time remaining in the transfer forward phase */ 


VX VY ~/* Velocity of cockpit in body coordinates ae 
| rel hd, /* Relative heading of cockpit velocity */ 
proj dist. /* Projected distance forward for new footholds * / 
| min time; /* Minimum time to reach any cwv limit */ 
int if 


vector cwv velocity, /* Instantaneous velocity of the center of 
the cwv (earth coodinates) */ 
time to limit, /* Time to reach the cwv limits */ 
bfh|7', /* Selected foothold in body coordinates * / 
bd footpos)7:: |* Desired foot position in body coordinates * / 


static vector d footpos.7: ;* Desired foot position in earth coordinates */ 


* foot trajectory ”* 
1 - leg number: 


if (“selected gait == FTL GAIT) 
if (transfer flag != ON) 
{ 
transfer flag 1) = ON; 
liftoff flagi = OFF; 
place flag|il = OFF; 
* Save foothold position. * / 
oldfia®x = fh atx; 
oldinyi.y = flay-y: 
oldfhji'.z = fhii'.z; 


proj dist = LENGTH * 0.21666: 


switch (1) 
{ 

case 1 * find new left foothold * 
VX = Grans Tate >Xx; 
Vy = trans rate->y + rot _rate->z ~ HALFLENGTH: 
rel hd = atan2(vy,vx): 
bfhgl ym 82.0; 
bfh 1.x = HALFLENGTH+ pro) dist*cos(rel_hd)-(82.0- 

proj dist*sin(rel hd))*tan(rel hd); 

bfh 1 .z = -160.0: 7 
/* ‘Transform to earth coordinates. */ 
Alfie he ibiha ts) 
transform point(fh,h,bfh,1); 
break: 

case 2: |* find new right foothold */ 
VX= trans rate->x; 
vy = trans rate->y + rot rate->z * HALFLENGTH; 
rel hd = atan2(vy,vx); 
bfh'2!.y = -82.0: 
bfh: 2.x = HALFLENGTH+ proj dist*cos(rel_hd)-(-82.0- 

proj dist*sin(rel hd))*tan(rel hd); 

bfh!2|.z = -160.0; 
/* Transform to earth coordinates. */ 
thi = oh "bibl ieie | 
transform point(fh,h,bfh,2); 
break: 

default: /* back leg uses old front leg foothold * 
fhii).x = oldfhji-2).x; 
fhany = oldfhji-2/.4; 
fh ij.z2 = oldfhji-2,.2; 
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Pwioot trajectory: 


_* determine the desired foot position *, 
d footposii|.x = fhlij.x; 
d footpos'i.y — finer: 
d footpos|i.z = fh'i.z; 


} 
else ‘FWD WAVE GAIT Calculate a new desired foot position 


at each time increment. */ 


/* Calculate the desired touchdown point. */ 


/* Future change note: Change from cwv center to midstance. */ 


/* Calculate foot velocity at center of cwv (body coordinates) */ 


cwv velocity.x = trans rate->x - rot rate->z * 


cwv(ij.y.center 
+ rot rate->y * cwv{il.z.center; 

cwv velocity.y = trans rate->y + rot rate->z ” cwv i'.x.center 
-rot rate->x * cwvji.z.center: 

cwv velocity.z = trans rate->z- rot rate->y * cwv/il.x.center 
+ rot rate->x * cwviil.y.center: 


/* Calculate the time to reach the limits of the cwv. * 
if (cwv velocity.x < 0.0) 


time to limit.x = ( cwvi).x.min- cwvi.x.center})/cwv velocity.x; 


else if (cw velocity.x > 0.0) 


time to limit.x = ( cwvii).x.max - cwvli}.x.center)/cwv velocity.x; 


else 


{ 
time to hmit.x = LONG TIME; 


} 


if (cwv_velocity.y < 0.0) 
time to limit.y = ( cwvil.y.min - cwvii.y.center)/cwv velocity.y; 


else if (cwv velocity.y > 0.0) 


time to limit.y = ( cwv'il.y.max - cwv'il.y.center) ‘cwv velocity.y; 


} 


else 


{ 
time to limit.y = LONG TIME, 
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foot trajectory ”* 


if (cw velocity.z < 0.0) 
time to limit.z = ( cwviij.z.min - cwv'l.z.center} cw velocity.z:; 
else if (cwv velocity.z > 0.0) 


time to limit.z = ( cwvii].z.max - cwviij.z.center)/cwv velocity.z; 


else 
{ 

time to hmit.z = LONG TIME: 
} 


x 


Determine the minimum time to reach the cwv limit. * 
min time = time to limit-x: 
if (time to limit.y < min time) 
min time = time to hmit.y: 
if (time to limit.z < min time} 
min line = vimMme vo linmit 2. 


x 


* Calculate the desired touchdown point in body coordinates. 
* Note: This point changes if the body is in motion. * 


bd footpos1.x = cwvii).x.center + cwv velocity.x *~ min time * .9; 
bd footpos1.y = cwviil.y.center + cwv velocity.y * min time * .9; 
bd footposi.z = cwvil.z.center ~— cwv velocity.z * min time * .9; 


Transform to Earth coodinates. *.’ 


* id footpos 1. T = h * :bd footposii]|/T */ 
transform point(d footpos,h,bd footpos,i); 


* Calculate the time remaining in the transfer forward phase. */ 


trans fwd time = “trans time * 


(“begin place phase - *trans_ phase); 


x 


Calculate the new foot position. (Earth Coordinates) * | 
if (DELTA TIME < trans fwd time} 
! 
\ 
footpos i).x += (d footpos 1.x - footpos'i.x) 
* DELTA TIME , trans fwd time; 
footpos 1.» += (d footpos il.y - footpos!i.y) 
* DELTA TIME / trans fwd time, 


footpos i|.2 = footpos.i).z; /* Level ground assumption! * 
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* 


foot trajectory * 


* 


else -* Last increment of time 
footpos'i.x = d footpos|i).x; 
footpos 1 y — d_ footpos|i|.y: 
foot pos|1i.z 


! 


fer. * : * 
footposi.z; /* Level ground assumption! */ 


mK * / 


Transform to body coodinates. 
ib footpos|i}/T = invh * 'footpos!i]/T */ 
transform point(b footpos,invh,footpos,1); 


3 


/* end of transfer trajectory 
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foot trajectory 
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placement trajectory({liftoff flag.place flag.transfer flag foot pos. 
b footpos.invh.trans time.trans phase. leg number) 


This function calculates the trajectory of the foot while it is 
being lowered from the ground. It is called from foot trajectory().*, 
on 
/ 


* 
x 


foot pos 7), Present foot position in earth coordinates 
Present foot position in body coordinates 


vector 
b footpos|7j; / 


/ 


float “trans time, 
*trans phase, 
“ Inverse homogeneous transformation matrix 


invh 4 4: 
int liftoff flag 7, ,* Indicates the first time entering subroutine 
in current leg cycle */ 


transfer flag 7). 
place flag7. 
leg number: 


float place time: 
7 


Int ot 
static vector d footpos'7.; /* Desired foot position in earth coordinates 


i = leg number. 
x 


‘Calculate the desired foot position. 
if ( place flagi, != ON) 
{ 

= footpos|i).z - FOOTLIFTHEIGHT; 


d foot pos,i}.z 
liftoff flagia = OFF; 
transfer flaglil = OFF; 


place flagi' — ON: 


j 
Calculate the time required to reach the desired height 


from the present foot position. */ 
place time = “trans time * (1.0- *trans_ phase); 


,* 


Calculate the new foot position. (Earth Coordinates) */ 


if (DELTA TIME < place time) 


footpos i .z += (d footpos.i..z - footpos i .z) 


* DELTA, TIME / place time; 
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* 


42 
, foot trajectory 


* 


else * Last increment of time 


{ 


footpos 1.2 = d footpos i).z: 


‘* Transform to body coodinates. *) 
/* |b footpos|i; T = invh * |footpos:i]:T */ 
transform point(b footpos,invh,footpos,i); 


} /* end of placement trajectory */ 
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support trajectory(liftoff flag,place flag,transfer flag footpos, 
b footpos.invh,leg number} 

* This function calculates the trajectory of the foot during the 

foot support phase. It is called from foot trajectory().*/ 

vector footpos7), |'* Present foot position in earth coordinates 

‘* Present foot position in body coordinates */ 


ay 


b foot pos 7 ; . 
float invhi44;  /* Inverse homogeneous transformation matrix * 


int liftoff flag 7, ,* Indicates the first time entering subroutine 
in current. leg cycle */ 
transfer flag 7, 
place flag7. 
leg number; 


int Me 


In this phase the foot is kept stationary on the ground. It 
* 


1 
i 


is assumed that the foot will not slip or move accidently. 


1 = leg number. 

* Transform foot position to body coodinates. 
b footposi T = invh* footpos1 T 

transform point(b foot pos.invh footpos,i); 


x 


‘x 


Turn off flags. */ 
hitefi flag 1 = OFF; 
transfer fag —=-OF fF: 
place flagji) = OFF; 


} /* end of support trajectory 


ee ee ee ee oe re a ee ky Rta aan Me oe oe ek oe er oe or OR oe a ok Ok Ee ok eK ok 2k ok oF 


This function is for the program walk.c on the iris-2400. 
conwalk.c 
Based in part on J.H.Kessler’s 
Rel Lyman program "conwalker.c" 
24 Apr 1987 


eg Oo ee ee ee ee ee Re ee A RE EE Ke A EE | 
/ 


#include "gl.h" 
#include "device.h" 
zinclude "walk.h" 


'* 


: This function calls up the walker from constructwalker (with legs 


already properly positioned) and then rotates and translates it as 

commanded. see 

‘* Note: Due to the limited number of bit planes available 
four separate walkers are constructed, one for each viewing 
quadrant. The walker for each quadrant is drawn from furthest 
component to nearest. This provides a quasi- Z buffer effect 
while in double buffer mode. lt 


{ 


makewalker(machineobject,d1,d2,theta.knee.gamma,alpha,transrot tag, 
tr end tag,walker,leg.thighob),actuatorobj,shinobj, 
legmovetag,thighmovetag.actmovetag,shinmovetag ,tx,ty,tz, 
roll,elev.azimuth,hx,hy,hz.14) 


Tag  transrot tag 4 .tr end tag 4 ,legmovetag 4, 
thighmovetag |\2 4 ,actmovetag|| 2/4 ,shinmovetag, 2.4; 


Object machineobject!4 ,walker|4|,thighobj |/2)/4),actuatorobj ||2!/4j, 


shinob)|] 2) 4!,leg |/4); 
int dl |,d2([|,knee/||2! ; 
Angle theta ,alpha,gamma|/; 


float tx,ty,tz,roll,azimuth,elev, 


hx 7|,hy|7|,h2{7],14{7]: 


int n; 
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* conwalk.c * 


} 


constructwalker(walker.d1,d2.knee.alpha.gamma.theta.leg,thighob), 
actuatorob).shinob).legmovetag.thighmovetag.actmovetag, 
shinmovetag. hx. hy,hz,|4) ; 


for (n=O: n<4:n+-~) |* Rotate and translate the walkers in each 


quadrant. */ 


{ 
machineobject n|=genobj(): 
makeob){machineobject n); 
pushmatrix() ; 


* Note: Each walker is built on the origin. Rotations are done 
before translating to the proper location. */ 
transrot tag n =gentag(); 
maketag(transrot tagin ); 


translate(tx.ty,tz); 
rotate((int) (elev * 573), Y°): 
rotate((int) (roll * 573).°X’ 
rotate((int) (azimuth * 57 
tr end tag n =gentag(): 
maketag(tr end tag n ); 


callobj(walker n)); 


popmatrix() : 
closeob)(); 
\ * end quadrant loop */ 
ae : 


end of makewalker 


y conwalk-c * 
ee * a a a ES a EE eR KR RK KEK KE ER 
makeground(groundobject) 


1K 


/*~ This function creates a checkerboard groundplane below the ASV object.” / 


Object *groundobject: 
{ 

Object squareobject; 

Tag transaqrtag; 


static int 
ground1|4 3 ={{1000,-500,0},{1000,500,0}, {-1000,500,0}, {-1000,-500,0} be 
ground2(4|/3/={ { 2000,-1000,0}, { 2000, 1000,0} ,{-2000, 1000,0} ,{-2000,-1000,0} e 
square 4 eA {0,-100,0},{0,0,0},{-100,0.0}, {-100,-100,0}}; 


int ee 
leat tx,ty- 


squareobject - genobj(); 
makeobj(squareobject): 
color( WHITE): 
polfi(4 square): 
closeob)(): 


*groundobject=genobj(): 
makeobj(*groundobject); 


* / 
/ 


color(RED); /* fill outer background squares 
polfi(4,ground2); 

color(GREEN); /* fillinner background squares */ 
polfi(4,ground1); 


for (i=0; 1<40: i++) 


{ 


for (j=. j<20; j)-- +) 
if ((i+j)%2 < 1) 


tx=(i-20)*(-100.0); 
ty=(j-10) *(-100.0); 
pushmatrix(); 
translate(tx,ty,0.0); 
callobj(squareobject); /* place the white squares */ 
popmatrix(): 
We end if */ 
fe, end for j 7/ 
} /* end fori */ 
closeobj()}: 
} /* end makeground */ 
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* conwalk.c ~ 


Se ee ee EE a ee 


constructwalker(walker.d1.d2,knee,alpha,gamma.theta,leg.thighobj, 
act uatorobj,shinobj,legmovetag,thighmovetag.actmovetag, 
shinmovetag,hx.hy,hz,14) 


-* This is where the walker is made. Here each part is assembled 
and then the parts are put together. This assembled walker is 
then rotated and translated in makewalker which is called by 
the main program. oT 


Tag legmovetag|'!4_ ,thighmovetag | 2)'4 ,actmovetag |/2)[4, 


i 


shinmovetag,|'2) 4/; 
Object walker 4 leg (4 thighobj , 2:4),actuatorobj 2/'4,,shinobj|| 2 4; 
int difid2" knees? |: 
Angle alpha’ .gamma_.theta ;; 


float hx 7 .hy'7j,h2(71, /* leg pivot position * 
47: 


\ 


Object body. head.eye,boxobj 7 : 


static float legx'7j)={0.0.155.0.155.0,0.0,0.0,-155.0,-155.0}, 
legy 7 ={0.0,82.0,-82.0,82.0,-82.0,82.0,-82.0}, 
legz.7 ~ {0.0,0.0,0.0,0.0,0.0,0.0,0.0}: 


COOrRd x.v:7. 
int 1,).k.n,legnum ; 


static int 
/* Coordinates for building the body of the asv */ 
black body/4''3'={ {206,50,22}, {206,-50,22},{206.-30,-101},{206,30,-101} }, 
lbodyarry|4) 3]={{-200,30,-101}, {-200.50.22}, {206,50,22},{206,30,-101}}, 
rbodyarry 4 [3 ={{-200,-30,-101}.{206,-30,-101},{206,-50,22},{-200,-50,22}}, 
tbody arry |/4)\[3:={ {-200,50,22}, {-200,-50,22}, {206,-50,22}, {206,50,22}}, 
bbodyarry/4|/3'={ { 206,-30,-101}, {-200,-30,-101},{-200,30.-101},{206.30,-101 } }, 
backbodyarry!4 3'={{-200,30,-101}, {-200,-30,-101}, {-200,-50,22}, {-200,50,22} }, 
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* * 


conwalk.c 
/* Coordinates for building the hydraulics housing structure * 

front rt top 4a|3)={{2%)-25,16},{38,-25,-13},{38,-13,-13},{27,-12,16}}, 
front rt bttm/4i(3j={{38,-25.-13} {38.-25.-46} {38,-13.-46},{38.-13.-13} }, 
rt_ interior 5'(3\={{20,-25,38},{38,-25,-13},{38.-25.-46}.{-38.-25,-46},{-38,-25,38}}, 
rt side(5 3.= { {-38,-25.38},{-38,-25,-46}, {38,-25,-46}, {38.-25.-13}.{20,-25,38}}, 

lt interior!5] S#={{-38,25,38} {-38,25.- 46},{38,25, -46} {38,25,- ne oneal: 
lt side[5|[3)/={{20. 25,38}, {38,25,-13},{38.25.-46},{-38,25.-46}, {-38.25.38} }, 
top_box 4) 3 ={{20,-25,38},{20,25,38},{-38,25,38},{-38,-25.38,}}, 

back box'413i=4 f-38,25,-46' 1-38 ,-25,-46},{-38,-25,38),{-38,25,38}}, 

front — ‘top 4) ire larpe eh 20s 2oeos en 20 lon 2a.20.13}), 

Hentelt top eer —44 20,130,160). 138 Ng atd),{o8\25,-13),127,25.16}}. 

front It bttm 4) 3) = ooo la aconl aie 46) 4io5,25.-40),138,25,-13} }, 

bttm_ t/4/[3) ={{38,25,-46},{38,13,-46},{-38,13,-46} ,{-38,25,-46}}, 
bttm rt|4) 3 ={{38.-25,-46},{38,-13,-46}, {-38,-13,-46},{-38,-25,-46} }, 


highbox top/4//3)={{-8,-25,88},{8,-25,88},{8,25,88}.{-8,25.88}}, 
highbox front|4]|3]={{8,25,88},{8,-25,88},{10,-25,38},{10,25,38}}, 
highbox eee a eee ge Wee ection 2278) 10, 2,88}}, 
highbox rt|4 [3'={{8,-25,88},{-8,-25.88}, {-10,-25,38}{10,-25,38}}, 
highbox It! 43 = { {-8.25,88}, {8.25.88}, {10,25,38}, {-10,25,38}}, 


noespar fromtidi|> —{479.-13,-20}.{79.-25,-20} {79. Be -30} ,{79,-13,-30}}, 
rt spar top 4 3 ={{79.-13,-20},{38.-13.-19}.{38,-2 519}, GG 25-20) 
rt spar bttm.4 |3 ={{38.-13,-32}.{79.-13. ee ve -30} ,{38,-25,-32}}, 
rt spar rt'd ° FP if23 05, ool {79,-25,-30},{ 25. mn {38,-25,-19}}, 
neespar eto) =4499)-15,-30),138,-13,-32). a Torte 9=19.-203 ), 

lt spar fronti4 3)={{79.25,-20},{79.13.-20},{79.13,-30},{79,25,-30}}, 

It spar top'4) 3)={{79,25.-20},{38,25,-19},{38,13,-19}.{79,13,-20}}, 

lt spar “buemd Pl alee 2on-82 ) 2489, 29,-50)44 19,1330), 138,13,-32) }, 

lt spar rt'4|/3)={{38,13,-32},{79.13,-30},{79,13,-20},{38,13,-19}}, 

It spar It/4!(3|={{79,25,-30},{38,25,-32},{38,25,-19},{79,25,-20}}, 
/* cab construction arrays */ 
cab aa Nl ne -30,-101},{206,-30,-101}, {206,30,-101},{305,30,-101}}, 
cab topi4)'3 ={{250,33,74},{206,33,74},{206,-33,74},{250,-33.74}}, 


cab fwd support 4)[3:={{305,30,-101}, {305,41,-16},{305,-41,-16},{305,-30,-101}}, 
cab fwd lower!4)3)={{305.41,-16},{318,48.8},{318,-48.8},{305.-41.-16}}, 

cab fwd ee ne |={{318,48.8} {302,33,68} ,{302.-33,68},{318.-48,8} }, 

cab fwd ~ovhd(4) Pelee eS) 250655014194 250,253,794 1.1275.-35,08} }, 


cab rt support '4)/3)={{305.-30,-101},{305,-41,-16} ,{206,-41.-16}, {206,-30,-101}}, 
cab rt lower 4 = 305, -41.-16},{318.-48.8},{206.-48,8} {206.-41.-16}}, 

cab rt upper| (4}|3)=4 1318, -48,8},{302,-33, 68}, {206,-33,68},{206,-48.8} }, 

cab rt ovhd(4]/3|= ae 5,-33,68} ,{ 250,-33,74},{206.-33,74},{206,-33,68}}, 
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*¥ 


* conwalk.c 


cab It support.4 3 {{206.30,-101},{206,41,-16},{305.41,-16},{305.30.-101}}. 
cab It lower 4 3 ~ {{206.41,-16},{206,48,8},{318,48.8},{205.41,-16}}, 
cab It upper 4 3 —{{206.48.8},{206.33,68},{302,33,68},{318.48.8}}, 
cab lt ovhd'4 3 ={{206.33,68},{206,33,74},{250,33.74}, {275,33,68}}, 


cab aft support.4 3:={{206.-30,-101},{206-41,-16}.{206,41,-16},{206,30,-101}}, 
cab aft lower/4: 3 ={{206,-41,-16},{206,-48,8},{206,48,8},{206,41,-16}}, 

cab aft _upper 4) '3'={{206,-48,8}, {206.-32.68}, {206,33,68}, {206,48,8}}, 

cab aft ovhd/4//3]={ {206,-33,68},{206,-33,74},{206,33,74},{206,33,68}}, 


scanner fwd lower:4]'3)={{302.33,68},{322,33,95} ,{322,-33,95} ,{302,-32,68}}, 
scanner fwd _upper|4 41(3;={ (322,33,95 },{222,33,101} (322,932 101 322 eee ee 
scanner rt[5)[3 ={{302,-33 + 1322,-99.99 | 1o22ese 101}, {275,-33,101) {275,32 ,08) qe 
scanner 1t'5|(3/={{302.33,68} {275.33 ce) {275,.33,101} {322 30550) 23 322503 Sona 
scanner aftj4/\3 ={{2 5. 38 Baal (oi oroe OS em 54-33 |68} 4 20 5,-so Ot) 

scanner top 4; 3,={{322.33,101},{275,33,101},{275,-33,101},{322,-33,101}}; 

* The making of the leg 1s quite complicated. Each leg consists of an 
upper link (thigh), lower link (actuator). and a shank (shin). These 
segments are first defined in a standardized orientation, and are then 
rotated and translated into the proper position. This is done by using 

2 objects for each segment. The first object is the correctly rotated 
segment, and the second object is the correctly translated first 

object. Thus the segment is then in the proper position. To hold the 
screen coordinate system fixed the matrix is pushed before each translation 
or rotation and then popped after the object is constructed or called. */ 

iy 


for (n—O; n<4, n+ +) Make a set of legs for each viewing quadrant 


Each quadrant must have unique tags. my 


{ 
for(legnum=1 ;legnum<7;legnum++} 
\ 
~ Each segment is constructed and positioned */ 
buildthigh(n,legnum,d1,alpha,thighobj,thighmovetag) ; 
buildactuator(n,legnum,d2,alpha,actuatorob),actmovetag) ; 
buildshin(n.legnum,knee,gamma,shinob),shinmovetag) ; 


leg legnum in, =genobj(); 
makeobj(leg legnum| n}); 
push matrix(); 
* translate(legx legnum|,legy legnum ,legz'legnum ) ; *, 
translate(hx|legnum,,hy'legnum',hz legnum ) ; 
legmovetag legnum 'n =gentag(); /* The leg is assembled from *, 
maketag(legmovetag legnum n}); /* its parts and the entire leg is */ 
/* then rotated to the proper angle. */ 
rotate(theta legnum ,X’):; 
translate(0.0,14 legnum),0.0); /* extend leg outward * 
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‘conwalk.c 


if (((n > 1)&&(legnum < 5)) 
((n < 2)&&(legnum > 4))) 
{ 
if (legnum > 4) ™ Reverse the back legs. 
{ 
pushmatrix(}; 
rotate(1800,'Z’); 


* Build the left side first. * 


x 


/ 


j 
color(BLACh): 
polfi(5,lt interior); 


color(GREEN);: 
polfi(5,lt side); 
polfi(4,front It top); 
polfi(4.front It bttm): 
polfi(4,bttm It), 


polfi(4,jt spar front); 
polfi(4,lt spar bttm); 
polfi(4.lt spar It); 
polfi(4,lt spar rt); 


color( BLUE); 
polfi(4,lt spar top): 


color(BLACh); 
polyi(4,lt spar rt): 


colorntC YAN) : 
callobj(thighobj'legnum: 1! nj); 
callobj(actuatorob) legnum (1 (n ); 
callobj(shinobj legnum 1 )n ); 


color(GREEN) ; 
polfi(4,rt_ spar front); 
polfi(4,rt spar bttm); 
polfi(4.rt spar It); 
polfi(4,rt spar rt); 


color(BLUE); 
polfi(4,rt_ spar top); 


color(GREEN); 
polfi(4,front rt bttm); 
polfi(4,front rt top); 
polfi(4,front top): 
polfi(4,bttm rt); 
polfi(4,back box): 
polfi(4.top box); 
polfi(5,rt side); 
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conwalk.c ” 


color(BLACh}; 
polyi(4.top box); 
polyi(5.rt_ side); 
polyi(4.rt spar rt); 


color(GREEN)}); 
polfi(4,highbox front); 
polfi(4 ,highbox It), 
polfi(4,highbox back); 
polfi(4.highbox rt); 
polfi(4,highbox top); 


color(BLACh); 
polyi({4,highbox top); 
polyi(4.highbox rt); 
polyi(4.highbox back): 


if (legnum > 4) — , * For reversing the back legs. *, 
{ 
popmatrix() ; 
} 
} 
else * Build the right side first. *. 
\ 
if (legnum - 4) '* Reverse the back legs. */ 
\ 


pushmatrix(): 
rotate(1800.°Z'): 
} 


color(BLACK); 


polfi(5,rt_ interior); 


color(GREEN), 
polfi(5,rt_side); 
polfi(4,front rt top); 
polfi(4,front rt bttm); 
polfi(4,bttm rt); 


polfi(4.rt spar bttm); 
polfi(4.rt spar rt); 
polfi(4.rt spar It); 
polfi(4.rt spar front); 


color(BLUE); 
polfi(4,rt_ spar top); 
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color{(BLACh); 
polyi(4.rt spar It); 


color(CYAN) ; 
callobj(thighobj legnum) 1/-n ); 
callobj({actuatorobj/legnum) 1! n/'); 


callobj(shinobj|legnum ‘1: n ); 


color(GREEN) ; 
polfi(4,back box); 
polfi(4.bttm It): 
polfi(4,front top); 
polfi(4,front It bttm); 
polfi(4.front It top): 


polfi(4.lt spar bttm): 
polfi(4,lt spar rt); 
polfi(4,lt spar It): 
polfi(4,lt spar front); 


coler( BLUE); 
polfi(4.dt spar top); 


color(GREEN): 
polfi(4.top box); 
polfi(5,]t side); 


color(BLACh), 
polyi(4,top box); 
polyi(5,It_ side); 
polyi(4,lt spar It); 


color(GREEN); 
polfi(4,highbox back); 
polfi(4,highbox rt): 
polfi(4.highbox front); 
polfi (4.highbox It): 
polfi(4,highbox top); 


color(BLACK); 
polyi(4,highbox top); 
polyi(4,highbox It); 
polyi(4,highbox front); 
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if (legnum > 4) * For reversing the back legs. 
{ 
popmatrix() ; 
j 
} 
popmatrix(); 


closeobj() : 
\ ,* end of leg loop */ 
\ /* end of quadrant loop */ 


body=genobj() : * The body is constructed */ 


makeobj( body}; 


color(LTYELLOW) ; 
polfi(4.lbodyarry) ; 
polfi(4,backbodvarry) ; 
polfi(4.bbodyarry) ; 
polfi(4.rbodvarry) ; 


color y ELL ON: 
polfi(4.tbodvyarry ); 


color(BLACKh) : 
polfi(4. blackbody) : 
closeob){) ; 


head =genob)() : * construct the head * 
makeobj(head) : 

color( YEL EON: 

polfi(4.cab top); 

polfi(4.cab fwd ovhd); 

polfi(4,cab rt ovhd): 

polfi(d4.cab It ovhd}); 

polfi(4,cab aft ovhd); 


color(BLACKh); 
polfi(4,cab bottom); 
polfi(4.cab fwd support); 
polfi(4.cab rt support); 
polfi(4,cab lt su pport); 
polfi(4.cab aft support); 


color(W HITE); 
polfi(4,cab fwd lower); 
polfi(4,cab rt lower); 
polfi(d.cab It lower): 
polfi(4,cab aft lower); 


* 


conwalk.c * 


color(WHITE) : 
polfi(d.cab fwd upper); 
polfi(d,cab rt upper); 
polfi(d4.cab It upper); 
polfi(d.cab aft upper); 


colon BLA Ch ) 
polfi(4.cab top): 
polyi(4,cab fwd lower); 
polyi(4,cab fwd upper); 
polyi(4.cab fwd ovhd); 
polyi(4.cab rt lower); 
polyi(4,cab rt upper); 
polyi(4,cab rt ovhd); 
polyi(4,cab_ It lower); 
polyi(4,cab It upper); 
polyi(4,cab It ovhd); 


closeobj() ; 


eve=genob)() : ‘* contruct the radar (eye) * 
makeobj(eye) : 


color(RED): 
polfi(4.scanner fwd upper); 
polfi(4.scanner fwd lower); 
polfi(5,scanner rt): 
polfi(S.scanner It); 
polfi(4,scanner aft): 


color(BLACK) ; 
polfi(d.scanner top); 


color(BLUE) : 


closeobj() : 


walker 0'=genob)(): /* assemble all the parts for quad I *, 
makeobj(walker/0)); »* back and right first */ 


callobj(leg|6 |0 ); 
callobj(leg|4 0 ); 
callobj(leg|2_'0 ); 
callobj(body); 
callobj(head); 
callobj(eye); 
callobj(leg|5 (0 ): 
callobj(leg'3''0 ); 
callobj(leg 1'\0 ); 


closeobj() ; 
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* conwalk.c 


{> 


walker, 1.=genob)(): assemble all the parts for quad I] * 
makeobj(walker I ): * front and right first * 
callobj(leg 2,,1 ): 
callobj(leg 4) 1 ): 
callobj(leg 6) 1 ): 
callobj(head): 
callobj(eve); 
callobj( body ); 
callobj(leg 1)/1)): 
callobj(leg 3.:1 ); 
callobj(leg 5 1): 
* assemble all the parts for quad III* 
* front and left first * 


walker 2 =genob)(): 
makeobj(walker 2 ): 

callobj(leg 1, 2)): 

callobj(leg 3, 2)): 

callobj(leg 5,2); 
callobj(head); 
callobj(eye): 
callobj(body ): 
callobj(leg 2/2 ): 
callobj(leg 4, 2 ): 
callobj(leg 6) 2 ): 
walker 3 =genobj(); * assemble all the parts for quad IV 
makeobj(walker 3 ): * back and left first */ 

callobj(leg 5. 3 ): 

callobj(leg 3) 3°): 

callobj(leg 1 (3.): 

callobj( body): 

callobj(head): 

callobj(eye): 

callobj(leg 6|'3 ); 

3); 
3); 


callobj(leg'4| 
callobj(leg 2! 


vo 
( 
m2 
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conwalk.c 


[RR OR OK Kk EE ok KE ok oe ok oR OK ok oe ok aE OR oe oe ok oR a ok oe ook aR oe ok ok ok ok ok ok ok ok OK OK ok KK ok OK x 


buildthigh({n,legnum,d1.alpha.thighobj .thighmovetag) 


/* this function constructs the thigh (upper link) and rotates, then 
translates it into the proper position ad 


Tag thighmovetag 2 4; 

int n,legnum,d1 | ; 

Angle alpha) ; 

Object thighobjj,}2|/4:; 

. 

static int 

thighltside'4|'3|={{0,10,7},{102,10,7},{102,10,-7},{0,10,-7}}, 
thighrtside/4|\3 ={{0,-10,-7},{102,-10,-7},{102,-10,7},{0.-10,7}}, 
thighfront| i4)/9 Bi —=110.= 1057 } 44 102.-10,7) (4, 102,10,7},10,10,7}}. 
thighbttm/4,/3|={{0,10,-7},{102,10,-7},{102,-10.-7},{0,-10,-7}}; 


2 


thighobj legnum 0. n pecnoy\): 
makeobj(thighobj legnum 0 n); 


pushmatrix() : 


thighmovetag legnum 0 /n—gentag(); /* rotate thigh */ 
maketag(thighmovetag legnum 0|.n): 
rotate(alpha legnum',’Y’) ; 


f(legnum -4) =” Build the left side first. */ 


{ 
eolor(Cy AN): 
polfi(4,thighbttm), 
polfi(4,thighltside); 
polfi(4 .thighrtside); 


color(RED); 
polfi(4,thighfront); 


color(BLACK); 
polyi(4,thighrtside); 
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CON walkie 


else * Build the right side first. *, 


\ 
color(CYAN), 
polfi(4,thighbttm); 
polfi(4,thighrtside); 
polfi(4 thighItside); 


color(RED): 
polfi(4,thighfront); 


color(BLACh); 


polyi(4.thighItside): 


popmatrix() ; 
closeobj() : 


thighobj legnum (1 n =genobj() ; 
makeobj(thighobj legnum) 1 n) ; 


pushmatrix() : 
thighmovetag legnum, 1 n =gentag{(); 
maketag(thighmovetag legnum) 1 n ); /* translate thigh */ 


translate(0.0,0.0,(float)(-dl legnum )) ; 


callob)(thighobj legnum '|0: nj): 
popmatrix() : 


closeobj() ; 
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* conwalk.c * 


PR KK KK KK ROR RRR KE ER RK KR KKK KR KKK KKK KKK KKK KKK KK ER KEKE KKK KKK KKK KK KA KK KKK 


buildact uator(n,legnum,d2,alpha,actuatorobj,actmovetag } 


/* construct the actuator (lower link) * / 


Tag actmovetag||'2!/4]; 
int n,legnum,d2_ ; 
Angle alpha’); 


Object actuatorob)!|/2]/4 ; 


{ 


static int actitside/4||3/={{0,10,5},{83,10,5}.{83,10.-5},{0,10,-5}}. 
actfront/4//3 ={ {0,-10,5}.{83,-10.5},{83.10.5},{0.10,5}} , 
actrtside[4|(3|={ {0,-10,-5},{83,-10,-5},{83.-10,5},{0,-10,5}}. 
actbttm|4|j3|={ {0,10,-5},{83,10,-5},{83,-10,-5},{0,-10,-5}} ; 


actuatorob) legnum 0’ n =genobj(): 
makeobj(actuatorob) legnum/{0''n ); 


pushmatrix(}: 


actmovetag legnum! 0) n =gentag(); 
maketag(actmovetag'legnum O]/n!); | /* rotate actuator *. 


rotate(alpha legnum|, Y’) : 


if(legnum>4) /* Build the left side first. */ 


{ 
color(CYAN); 


polfi(4,actbttm); 
polfi(4,actltside); 
polfi(4.actrtside); 


color(RED); 
polfi(4.actfront): 


color(BLACK); 
polyi(4,actrtside); 
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* 


conwalk.c ” 
else /* Build the right side first. * 


color(CY AN); 

polfi{4,actbttm):; 
polfi(4,actrtside); 
polfi(4.actltside); 


color(RED); 
polfi(4,actfront); 


colori BLACK), 
polyi(4,actltside); 
j 
popmatrix():; 
closeobj(); 
actuatorobj legnum ! n ~genobj(): 


{ 


makeobj(actuatorobj legnum |1 in ); 


pushmatrix(): 
actmovetag legnum 1//n =gentag(): 
maketag(actmovetag legnum| 1 in ); * translate actuator */ 
translate((float)(d2 legnum) ),0.0,(float)(-L3)); 
callobj(actuatorobj legnum: 0. n ): 
popmatrix():; 
closeobj({); 
* end of buildactuator * 
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* conwalk.c 


he ok koe ot ed YEO i oe eK Konica icici se ease ice ck oko ic ok ye kee ek OF A OK ok ok Gk Sk oe) Ske ok ok Ok Si oe ok ak ae ok 


buildshin(n,legnum,knee,gamma,shinob},shinmovetag) 
* construct the shank (shin ) * 


Tag shinmovetag||!2]|4 : 
int n,legnum,knee | 2.; 
Angle gamma |; 
Object shinobj!) 2.4); 
{ 

Static int 
shinltside|6|'3|={ {6.5.3},{ 10,5,-59},{-7,5,-50} ,{-6,5,3}.4-3,5.6},{3.5,6}}, 
shankltside 4]/3)={{10,5,-59},{-23,5,-102},{-36,5,-100},{-7,5,-30}} , 
shinfront'4||3 ={{6,5,3},{6,-5,3},{10.-5,-59},{10,5,-59}}, 
shankfront|4,/3|={{10,-5,-59},{-23,-5,-102}, {-23.5,-102}.{7,5,-59}}, 
ankleltside|/6 |3'={{-23.5,-102},{3.5,-153},{2.5,-157},{-3,5,-158},{-6.5.-158} {-36,5,-100}}, 
shinrtside 6|/3'={ {3,-5,6},{-3.-5,6}.{-6,-5.3} ,{-7,-5. "50}.{10,-5.-60}.{6,-5,8}), 
shankrtside|4/3)={{-7,-5,-50}, {-36, - 100} ,{-23,-5,-102},{10,-5,-59}} , 


anklertside 6) sl={{. 36,-5,- 100} ,{-6.-5,-158},{-3 Be nerd 12,-5,- a Wonder aowoen,- 102). 
anklefront'4 | ane ee 102}, {- oe HOZ 43-01 59).135.5,-153} 

shinback|4| [3 l={{ 3-90} (4-0 ,- 9501, 11-0, 000 104 - f b- 50 t 

shankback'4 3 = on -5,-100},{-7,-5,-50} ,{-7.5,-50},{-36,5.-100}}, 


ankleback|4!/3 He 6,-5,-158} ,{-36,-5,-100},{- sa 100}, {-6,5,-158}}, 
bottom fwd/4!3!={{3,5,-153},{2, 5-157}, {2,-5,-157},{3,-5,-153}}, 
bottom “mid/4) isi={{2,5,-157}, {-3,5,-158}, {-3 755 loon 2 =o lon}, 
bottom aft/4}[3]={{-3,5,-158},{-6,5,-158},{-6,-5,-158},{-3,-5,-158}}; 


shinobj/legnum|{0)[n|=genobj(); 
makeobj(shinobj/legnum) 0 n_ ); 
pushmatrix() ; 


shinmovetag legnum||0 n|=gentag(); 
maketag(shinmovetag legnum||0|/n|);_ /* rotate shank */ 
rotate(gamma legnum ,’Y’); 


if(legnum>4) /* Build the left side first. */ 


/ 


color(BLACK); 
polfi(4,bottom fwd): 
polfi(4,bottom mid); 
polfi(4,bottom aft); 


color(CY AN); 
polfi(4,ankleback); 
polfi(6,ankleltside); 
polfi(6,anklertside); 


color(RED); 
polfi(4,anklefront) ; 
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* conwalk.c * 


color(CY AN); 

polfi(4.shankback) ; 
polfi(4.shankltside}; 
polfi(4.shankrtside}; 


color(RED), 
polfi(4.shankfront) ; 


color(CY AN): 

polfi(4.shinback}; 
polfi(6.shinltside}; 
polfi(6.shinrtside); 


color(RED)}, 
polfi(4.shinfront) ; 


color(BLACK): 
polyi(6,anklertside); 
polyi(4.shankrtside); 
polyi(6.shinrtside); 


else /* Build the right side first. */ 


color(BLACK), 

polfi{(4.bottom fwd) ; 
polfi(4.bottom mid) ; 
polfi(4,bottom aft) 


color(CYAN}; 
polfi(4,ankleback); 
polfi(6.anklertside}; 
polfi(6,ankleltside); 


color(RED}); 
polfi(4.anklefront); 


color(CY AN); 

polfi(4,shankback) ; 
polfi(4,shankrtside}, 
polfi(4,shankltside}; 


color{RED); 
polfi(4,shankfront); 


color(CY AN); 

polfi(4.shinback); 
polfi(6.shinrtside}); 
polfi(6,shinltside}); 
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color(RED): 
polfi(4,shinfront): 


color(BLACK); 
polyi(6,ankleltside); 
polyi(4,shankItside}), 
polyi(6,shinItside); 
color(BLACK); 


pushmatrix({); 
rotate(-900,'X’); 
translate(0.0,0.0,5.0); 
eirct(0.0, 0.01700) 
Circ Or Us2 0a.) 
popmatrix{): 


pushmatrix(); 
rotate(900.’X°"): 
translate(0.0,0.0,5.0); 
circf(0.0.0.0,7.0) ; 
emenO0-52,0.5.0) ; 
popmatrix(); 


popmatrix(); 
closeobj(}; 


shinobj'legnum '1/|n|=genobj(); 
makeobj(shinobj legnum| Ln ); 


pushmatrix(); 

shinmovetag|legnum_ 1|/n|=gentag(); 

maketag(shinmovetag legnum_ 1){n}); /* translate shank */ 
translate((float)knee legnum| 0],0.0,(float)kneejlegnum|{1)) ; 
callobj(shinobj legnum|/0 /n ): 

popmatrix(); 


closeobj(); 


\ -* end of buildshin */ 
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This is a function for the iris2400 program walk.c. 
toolbox.c 


Relle Lyman 25 Aug 1986 
OR OK EO OR OK eK OK KOR KO OK OK OK OK KK OK OR OK OK OK KK OK KO KOK KOK KO KOK OK KK KK OK 
include '"g].h" 
#include "device. h" 
FF itic lide: vw alikeh’! 
+#include <stdio.h> 
#include <math.h> 


ee a een ee ee eee 


transform point(p2.m,p1.1) 


jm 


/* This function changes the coordinate system for a point vector 


ac 


using a homogeneous transformation submatrix. p2=m*pl *, 


iL ae _* Leg number * 
float m 4)4); ‘* Homogeneous transformation submatrix * / 


vector pl 7)... * Vector represented in first coordinate system ~*/ 
4; 


p2 »* Vector represented in transformed coordinate system 


2i.x = m(O![Ol*ptii;.x + m O]:1|/*pllij.y + m{0]|2)*pl{i .z — m(0]|3!: 
p2ii-y = m([1'(Ol*pljii.x + m 1\[1]*plil-y + mi1/{2:*ptli .z + m[t)/3 
p2 i.z = m 2!!0|*pl-al.x + mj2||1|*pl i,.y + m[2](2|*ptruj.z + mi2il3 


) 
{ 
* 


x 


} ,* end of transform point 


[RRR OR KK KK OK Ok KOR OR RK KK RK KK KR KO KK RR KK KK RK RK KK RK KK RK KK KR KR KKK KEE KK KK KR KKK KK KK / 
f 


float modulus one(temp) 


4 


cy 


/ 


This function performs the modulus one operation on numbers of type float. 


float temp; 


{ 


while (temp >= 1.0) 


{ 
temp -= 1.0; 


while (temp < 0.0) 


{ 
(emp = = 1-0. 


return Lemp: 
} = * end of modulus one */ 


* Makefile *, 


# This is Makefile. It is used in the utility make to speed 
# compilation of walk.c. To use it, just type "make". 


CFLAGS = -Zf -Zg -g 


Shes: = walk.c 


conwalk.c 
support.c 
toolbox.c 
steering.c 
body rates.c 
ft traj.c 

Opt period.c 
leg phase.c 
con. work vol.c 
driver.c 
status.c 
decelerate.c 
initee 


OBIS = walk.o 


walk : 


conwalk.o 
support.o 
toolbox.o 
steering.o 
body rates.o 
ft traj.o 

opt period.o 
leg phase.o 
con work vol.o 
driver.o 
Status.o 
decelerate.o 
init.o 


(OBJS) 
cc -o walk (OBJS) -Zg -Zf 


(OBJS): = walk.h 
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